From 8e5758ce8824a884232845793bbcb382504f5e76 Mon Sep 17 00:00:00 2001 From: martha-johnston Date: Tue, 21 Oct 2025 17:58:54 -0400 Subject: [PATCH 01/27] pause --- .../lib/resources/arm_screen.dart | 31 ++ .../arm_widgets/angular_arrows_widget.dart | 380 ++++++++++++++++++ .../arm_widgets/joint_positions_widget.dart | 180 +++++++++ .../arm_widgets/linear_arrows_widget.dart | 242 +++++++++++ .../arm_widgets/orienation_widget.dart | 228 +++++++++++ .../arm_widgets/position_widget.dart | 203 ++++++++++ .../viam_example_app/lib/robot_screen.dart | 59 ++- 7 files changed, 1307 insertions(+), 16 deletions(-) create mode 100644 example/viam_example_app/lib/resources/arm_screen.dart create mode 100644 example/viam_example_app/lib/resources/arm_widgets/angular_arrows_widget.dart create mode 100644 example/viam_example_app/lib/resources/arm_widgets/joint_positions_widget.dart create mode 100644 example/viam_example_app/lib/resources/arm_widgets/linear_arrows_widget.dart create mode 100644 example/viam_example_app/lib/resources/arm_widgets/orienation_widget.dart create mode 100644 example/viam_example_app/lib/resources/arm_widgets/position_widget.dart diff --git a/example/viam_example_app/lib/resources/arm_screen.dart b/example/viam_example_app/lib/resources/arm_screen.dart new file mode 100644 index 00000000000..a05c1d3d9dd --- /dev/null +++ b/example/viam_example_app/lib/resources/arm_screen.dart @@ -0,0 +1,31 @@ +import 'package:flutter/material.dart'; +import 'package:viam_example_app/resources/arm_widgets/joint_positions_widget.dart'; +import 'package:viam_example_app/resources/arm_widgets/linear_arrows_widget.dart'; +import 'package:viam_example_app/resources/arm_widgets/orienation_widget.dart'; +import 'package:viam_example_app/resources/arm_widgets/position_widget.dart'; +import 'package:viam_sdk/viam_sdk.dart'; + +/// A widget to control an [Arm]. +class ViamArmWidgetNew extends StatelessWidget { + /// The [Arm]Expand commentComment on line R9ResolvedCode has comments. Press enter to view. + final Arm arm; + + const ViamArmWidgetNew({ + super.key, + required this.arm, + }); + + @override + Widget build(BuildContext context) { + return Column( + children: [ + JointPositionsWidget(arm: arm), + LinearArrowsWidget(), + PositionWidget(arm: arm), + // AngularArrowsWidget(), + // JointPositionsWidget(arm: arm), + OrientationWidget(arm: arm), + ], + ); + } +} diff --git a/example/viam_example_app/lib/resources/arm_widgets/angular_arrows_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/angular_arrows_widget.dart new file mode 100644 index 00000000000..9015381db1e --- /dev/null +++ b/example/viam_example_app/lib/resources/arm_widgets/angular_arrows_widget.dart @@ -0,0 +1,380 @@ +import 'package:flutter/material.dart'; + +final size = 300.0; + +class AngularArrowsWidget extends StatelessWidget { + const AngularArrowsWidget({super.key}); + + @override + Widget build(BuildContext context) { + return Column( + children: [ + Divider(), + Text( + 'End-effector Orientation', + style: TextStyle( + fontWeight: FontWeight.bold, + ), + ), + Divider(), + SizedBox(height: 40), + Stack( + children: [ + _CurvedArrowPad( + // TODO: add functions for arrow functionality + onUp: () {}, + onDown: () {}, + onLeft: () {}, + onRight: () {}, + ), + _BuildCornerButton( + direction: ArcDirection.left, + label: 'RZ+', + onPressed: () {}, + ), + _BuildCornerButton( + direction: ArcDirection.right, + label: 'RZ-', + onPressed: () {}, + ), + ], + ), + ], + ); + } +} + +enum ArrowDirection { up, down, left, right } + +enum ArcDirection { left, right } + +class _Corners { + final Offset topLeft; + final Offset topRight; + final Offset bottomRight; + final Offset bottomLeft; + + _Corners({ + required this.topLeft, + required this.topRight, + required this.bottomRight, + required this.bottomLeft, + }); +} + +class _CurvedArrowPad extends StatelessWidget { + final VoidCallback? onUp; + final VoidCallback? onDown; + final VoidCallback? onLeft; + final VoidCallback? onRight; + + const _CurvedArrowPad({ + this.onUp, + this.onDown, + this.onLeft, + this.onRight, + }); + + @override + Widget build(BuildContext context) { + return Center( + child: SizedBox( + height: size, + width: size, + child: Stack( + children: [ + _BuildArrowButton(alignment: Alignment.topCenter, direction: ArrowDirection.up, onPressed: onUp, label: 'RX-'), + _BuildArrowButton(alignment: Alignment.bottomCenter, direction: ArrowDirection.down, onPressed: onDown, label: 'RX+'), + _BuildArrowButton(alignment: Alignment.centerLeft, direction: ArrowDirection.left, onPressed: onLeft, label: 'RY-'), + _BuildArrowButton(alignment: Alignment.centerRight, direction: ArrowDirection.right, onPressed: onRight, label: 'RY+'), + ], + ), + ), + ); + } +} + +class _BuildArrowButton extends StatelessWidget { + final Alignment alignment; + final ArrowDirection direction; + final String label; + final VoidCallback? onPressed; + + const _BuildArrowButton({ + required this.alignment, + required this.direction, + required this.onPressed, + required this.label, + }); + + @override + Widget build(BuildContext context) { + return Align( + alignment: alignment, + child: SizedBox( + width: size / 2.5, + height: size / 2.5, + child: IconButton( + icon: Stack( + alignment: Alignment.center, + children: [ + CustomPaint( + painter: _AngularArrowPainter(direction: direction, color: Colors.black), + child: const SizedBox.expand(), + ), + Text( + label, + style: const TextStyle( + color: Colors.white, + fontWeight: FontWeight.bold, + fontSize: 16, + ), + ), + ], + ), + onPressed: onPressed, + ), + ), + ); + } +} + +class _AngularArrowPainter extends CustomPainter { + final ArrowDirection direction; + final Color color; + + _AngularArrowPainter({required this.direction, required this.color}); + + @override + void paint(Canvas canvas, Size size) { + final paint = Paint() + ..color = color + ..style = PaintingStyle.fill; + + final path = Path(); + final w = size.width; + final h = size.height; + + final double offset = 0.2; + final double offsetInverse = 0.8; + + final double length = 0.3; + final double curveFactor = 0.1; + + final double curvePos = offset + curveFactor; + final double pointPos = offset + length; + + final double tipCurvePosInverse = offsetInverse - curveFactor; + final double headPosInverse = offsetInverse - length; + + switch (direction) { + case ArrowDirection.up: + path.moveTo(w / 2, h * offset); + path.quadraticBezierTo(w * (1 - curveFactor), h * curvePos, w, h * pointPos); + + path.lineTo(w * 0.8, h * pointPos); + path.lineTo(w * 0.8, h); + path.lineTo(w * 0.2, h); + path.lineTo(w * 0.2, h * pointPos); + path.lineTo(0, h * pointPos); + + path.quadraticBezierTo(w * curveFactor, h * curvePos, w / 2, h * offset); + break; + case ArrowDirection.down: + path.moveTo(w / 2, h * offsetInverse); + + path.quadraticBezierTo(w * curveFactor, h * tipCurvePosInverse, 0, h * headPosInverse); + + path.lineTo(w * 0.2, h * headPosInverse); + path.lineTo(w * 0.2, 0); + path.lineTo(w * 0.8, 0); + path.lineTo(w * 0.8, h * headPosInverse); + path.lineTo(w, h * headPosInverse); + + path.quadraticBezierTo(w * (1 - curveFactor), h * tipCurvePosInverse, w / 2, h * offsetInverse); + break; + case ArrowDirection.left: + path.moveTo(w * offset, h / 2); + + path.quadraticBezierTo(w * curvePos, h * curveFactor, w * pointPos, 0); + + path.lineTo(w * pointPos, h * 0.2); + path.lineTo(w, h * 0.2); + path.lineTo(w, h * 0.8); + path.lineTo(w * pointPos, h * 0.8); + path.lineTo(w * pointPos, h); + + path.quadraticBezierTo(w * curvePos, h * (1 - curveFactor), w * offset, h / 2); + break; + case ArrowDirection.right: + path.moveTo(w * offsetInverse, h / 2); + + path.quadraticBezierTo(w * tipCurvePosInverse, h * (1 - curveFactor), w * headPosInverse, h); + path.lineTo(w * headPosInverse, h * 0.8); + path.lineTo(0, h * 0.8); + path.lineTo(0, h * 0.2); + path.lineTo(w * headPosInverse, h * 0.2); + path.lineTo(w * headPosInverse, 0); + + path.quadraticBezierTo(w * tipCurvePosInverse, h * curveFactor, w * offsetInverse, h / 2); + break; + } + path.close(); + canvas.drawPath(path, paint); + } + + @override + bool shouldRepaint(covariant _AngularArrowPainter oldDelegate) => false; +} + +class _BuildCornerButton extends StatelessWidget { + final ArcDirection direction; + final String label; + final VoidCallback onPressed; + + const _BuildCornerButton({ + required this.direction, + required this.label, + required this.onPressed, + }); + + @override + Widget build(BuildContext context) { + return Align( + alignment: Alignment.center, + child: Padding( + padding: const EdgeInsets.all(24.0), + child: SizedBox( + width: 100, + height: 100, + child: Stack( + children: [ + CustomPaint( + painter: _ArcArrowPainter(sign: direction == ArcDirection.left ? -1 : 1, label: label), + child: const SizedBox.expand(), + ), + ], + ), + ), + ), + ); + } +} + +(Offset, Offset) _getControlPoints(_Corners corners, double sign) { + const double arcHeight = 45.0; + + final topChordLength = (corners.topRight - corners.topLeft).distance; + final bottomChordLength = (corners.bottomRight - corners.bottomLeft).distance; + final bottomArcHeight = arcHeight * (bottomChordLength / topChordLength); + + final topMidpoint = Offset( + (corners.topLeft.dx + corners.topRight.dx) / 2, + (corners.topLeft.dy + corners.topRight.dy) / 2, + ); + final topPerpendicular = Offset(corners.topRight.dy - corners.topLeft.dy, -(corners.topRight.dx - corners.topLeft.dx)) * sign; + final topNormal = _normalize(topPerpendicular); + final topControlPoint = topMidpoint + topNormal * arcHeight; + + final bottomMidpoint = Offset( + (corners.bottomRight.dx + corners.bottomLeft.dx) / 2, + (corners.bottomRight.dy + corners.bottomLeft.dy) / 2, + ); + final bottomPerpendicular = + Offset(-(corners.bottomLeft.dy - corners.bottomRight.dy), corners.bottomLeft.dx - corners.bottomRight.dx) * sign; + final bottomNormal = _normalize(bottomPerpendicular); + final bottomControlPoint = bottomMidpoint + bottomNormal * bottomArcHeight; + + return (topControlPoint, bottomControlPoint); +} + +(Offset, Offset, Offset) _getArrowPoints(_Corners corners, double sign) { + const double arrowheadLength = 40.0; + const double shoulderWidth = 40.0; + + final endMidpoint = Offset((corners.topRight.dx + corners.bottomRight.dx) / 2, (corners.topRight.dy + corners.bottomRight.dy) / 2); + final endVector = corners.bottomRight - corners.topRight; + final endDirection = _normalize(endVector); + + final outwardVector = Offset(endVector.dy, -endVector.dx) * sign; + final outwardNormal = _normalize(outwardVector); + + final arrowPoint = endMidpoint + outwardNormal * arrowheadLength; + final shoulderTop = endMidpoint - endDirection * shoulderWidth; + final shoulderBottom = endMidpoint + endDirection * shoulderWidth; + + return (arrowPoint, shoulderTop, shoulderBottom); +} + +Offset _getTextPosition(Offset shoulderTop, Offset shoulderBottom, double width, double height) { + final midpoint = Offset((shoulderTop.dx + shoulderBottom.dx) / 2, (shoulderTop.dy + shoulderBottom.dy) / 2); + return Offset(midpoint.dx - width / 2, midpoint.dy - height / 2); +} + +Offset _normalize(Offset o) { + final d = o.distance; + if (d == 0) return Offset.zero; + return Offset(o.dx / d, o.dy / d); +} + +class _ArcArrowPainter extends CustomPainter { + final double sign; + final String label; + _ArcArrowPainter({required this.sign, required this.label}); + + @override + void paint(Canvas canvas, Size size) { + canvas.translate(size.width / 2, 0); + + final fillPaint = Paint() + ..color = Colors.black + ..style = PaintingStyle.fill; + + // Corners of the arc rectangle + final corners = _Corners( + topLeft: Offset(sign * 5, -50), + topRight: Offset(sign * 165, 30), + bottomRight: Offset(sign * 133, 54), + bottomLeft: Offset(sign * 5, -10), + ); + + final textPainter = TextPainter( + text: TextSpan( + text: label, + style: TextStyle(color: Colors.white, fontWeight: FontWeight.bold, fontSize: 16), + ), + textAlign: TextAlign.center, + textDirection: TextDirection.ltr, + )..layout(); + + Offset topControlPoint; + Offset bottomControlPoint; + Offset arrowPoint; + Offset shoulderTop; + Offset shoulderBottom; + Offset textPosition; + + (topControlPoint, bottomControlPoint) = _getControlPoints(corners, sign); + (arrowPoint, shoulderTop, shoulderBottom) = _getArrowPoints(corners, sign); + textPosition = _getTextPosition(shoulderTop, shoulderBottom, textPainter.width, textPainter.height); + + // Draw arrows + final path = Path() + ..moveTo(corners.topLeft.dx, corners.topLeft.dy) + ..quadraticBezierTo(topControlPoint.dx, topControlPoint.dy, corners.topRight.dx, corners.topRight.dy) + ..lineTo(shoulderTop.dx, shoulderTop.dy) + ..lineTo(arrowPoint.dx, arrowPoint.dy) + ..lineTo(shoulderBottom.dx, shoulderBottom.dy) + ..lineTo(corners.bottomRight.dx, corners.bottomRight.dy) + ..quadraticBezierTo(bottomControlPoint.dx, bottomControlPoint.dy, corners.bottomLeft.dx, corners.bottomLeft.dy) + ..close(); + + canvas.drawPath(path, fillPaint); + textPainter.paint(canvas, textPosition); + } + + @override + bool shouldRepaint(covariant CustomPainter oldDelegate) { + return false; + } +} diff --git a/example/viam_example_app/lib/resources/arm_widgets/joint_positions_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/joint_positions_widget.dart new file mode 100644 index 00000000000..5e83882555d --- /dev/null +++ b/example/viam_example_app/lib/resources/arm_widgets/joint_positions_widget.dart @@ -0,0 +1,180 @@ +import 'package:flutter/material.dart'; +import 'package:flutter/services.dart'; +import 'package:viam_sdk/viam_sdk.dart'; + +class JointPositionsWidget extends StatefulWidget { + final Arm arm; + const JointPositionsWidget({super.key, required this.arm}); + + @override + State createState() => _JointPositionsWidgetState(); +} + +class _JointPositionsWidgetState extends State { + List _startJointValues = []; + + @override + void initState() { + super.initState(); + _getJointInfo(); + } + + Future _getJointInfo() async { + // _startJointValues = await widget.arm.jointPositions(); + _startJointValues = List.generate(6, (index) { + return 0.0; + }); + setState(() {}); + } + + @override + Widget build(BuildContext context) { + return Column( + children: [ + Divider(), + Text( + 'Joint Angles', + style: TextStyle( + fontWeight: FontWeight.bold, + ), + ), + Divider(), + Padding( + padding: const EdgeInsets.all(16.0), + child: Column( + mainAxisSize: MainAxisSize.min, + children: _startJointValues.isEmpty + ? [CircularProgressIndicator.adaptive()] + : List.generate(_startJointValues.length, (index) { + return _BuildJointControlRow(index: index, arm: widget.arm, startJointValues: _startJointValues); + }), + ), + ), + ], + ); + } +} + +class _BuildJointControlRow extends StatefulWidget { + final int index; + final Arm arm; + final List startJointValues; + const _BuildJointControlRow({required this.index, required this.arm, required this.startJointValues}); + + @override + State<_BuildJointControlRow> createState() => _BuildJointControlRowState(); +} + +class _BuildJointControlRowState extends State<_BuildJointControlRow> { + static const double _minPosition = 0.0; + static const double _maxPosition = 180.0; + + List _jointValues = []; + List _textControllers = []; + + @override + void initState() { + _jointValues = widget.startJointValues; + _textControllers = List.generate( + _jointValues.length, + (index) => TextEditingController(text: _jointValues[index].toStringAsFixed(1)), + ); + super.initState(); + } + + @override + void dispose() { + for (final controller in _textControllers) { + controller.dispose(); + } + super.dispose(); + } + + void _updateJointValue(int index, double value) { + final clampedValue = value.clamp(_minPosition, _maxPosition); + + setState(() { + _jointValues[index] = clampedValue; + final formattedValue = clampedValue.toStringAsFixed(1); + if (_textControllers[index].text != formattedValue) { + _textControllers[index].text = formattedValue; + _textControllers[index].selection = TextSelection.fromPosition( + TextPosition(offset: _textControllers[index].text.length), + ); + } + }); + } + + @override + Widget build(BuildContext context) { + return Padding( + padding: const EdgeInsets.symmetric(vertical: 8.0), + child: Row( + children: [ + SizedBox( + width: 30, + child: Text( + 'J${widget.index + 1}', + style: Theme.of(context).textTheme.titleMedium, + ), + ), + Expanded( + child: SliderTheme( + data: SliderThemeData( + activeTrackColor: Colors.black, + inactiveTrackColor: Colors.grey, + thumbColor: Colors.black, + overlayColor: Colors.transparent, + showValueIndicator: ShowValueIndicator.never, + ), + child: Slider( + value: _jointValues[widget.index], + min: _minPosition, + max: _maxPosition, + divisions: (_maxPosition - _minPosition).toInt(), + onChanged: (newValue) => _updateJointValue(widget.index, newValue), + ), + ), + ), + SizedBox( + width: 70, + child: TextField( + controller: _textControllers[widget.index], + textAlign: TextAlign.center, + keyboardType: const TextInputType.numberWithOptions(decimal: true), + inputFormatters: [ + FilteringTextInputFormatter.allow(RegExp(r'^\d+\.?\d{0,1}')), + ], + style: const TextStyle(color: Colors.black), + cursorColor: Colors.black, + decoration: const InputDecoration( + border: OutlineInputBorder(), + focusedBorder: OutlineInputBorder( + borderSide: BorderSide(color: Colors.black), + ), + contentPadding: EdgeInsets.symmetric(horizontal: 8), + ), + onSubmitted: (newValue) { + final parsedValue = double.tryParse(newValue) ?? _jointValues[widget.index]; + _updateJointValue(widget.index, parsedValue); + }, + ), + ), + const SizedBox(width: 8), + IconButton( + icon: const Icon(Icons.remove), + onPressed: () { + _updateJointValue(widget.index, _jointValues[widget.index] - 1.0); + }, + ), + IconButton( + icon: const Icon(Icons.add), + onPressed: () { + _updateJointValue(widget.index, _jointValues[widget.index] + 1.0); + }, + ), + ], + ), + ); + } +} diff --git a/example/viam_example_app/lib/resources/arm_widgets/linear_arrows_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/linear_arrows_widget.dart new file mode 100644 index 00000000000..17afa1a3434 --- /dev/null +++ b/example/viam_example_app/lib/resources/arm_widgets/linear_arrows_widget.dart @@ -0,0 +1,242 @@ +import 'package:flutter/material.dart'; + +final size = 300.0; + +class LinearArrowsWidget extends StatelessWidget { + const LinearArrowsWidget({super.key}); + + @override + Widget build(BuildContext context) { + return Column( + children: [ + Divider(), + Text( + 'End-effector Position', + style: TextStyle( + fontWeight: FontWeight.bold, + ), + ), + Divider(), + Stack( + children: [ + _SlantedArrowPad( + // TODO: add functions for arrow functionality + onUp: () {}, + onDown: () {}, + onLeft: () {}, + onRight: () {}, + ), + _BuildCornerButton( + alignment: Alignment.topLeft, + direction: ArrowDirection.up, + label: 'Z+', + onPressed: () {}, + ), + _BuildCornerButton( + alignment: Alignment.topRight, + direction: ArrowDirection.down, + label: 'Z-', + onPressed: () {}, + ), + ], + ), + ], + ); + } +} + +class _BuildCornerButton extends StatelessWidget { + final Alignment alignment; + final ArrowDirection direction; + final String label; + final VoidCallback onPressed; + + const _BuildCornerButton({ + required this.alignment, + required this.direction, + required this.label, + required this.onPressed, + }); + + @override + Widget build(BuildContext context) { + return Align( + alignment: alignment, + child: Padding( + padding: const EdgeInsets.all(24.0), + child: SizedBox( + width: 100, + height: 100, + child: IconButton( + icon: Stack( + alignment: Alignment.center, + children: [ + CustomPaint( + painter: _LinearArrowPainter(direction: direction, color: Colors.black), + child: const SizedBox.expand(), + ), + Text( + label, + style: const TextStyle( + color: Colors.white, + fontWeight: FontWeight.bold, + fontSize: 16, + ), + ), + ], + ), + onPressed: onPressed, + ), + ), + ), + ); + } +} + +enum ArrowDirection { up, down, left, right } + +class _LinearArrowPainter extends CustomPainter { + final ArrowDirection direction; + final Color color; + + _LinearArrowPainter({required this.direction, required this.color}); + + @override + void paint(Canvas canvas, Size size) { + final paint = Paint() + ..color = color + ..style = PaintingStyle.fill; + + final path = Path(); + final w = size.width; + final h = size.height; + + switch (direction) { + case ArrowDirection.up: + path.moveTo(w / 2, 0); + path.lineTo(w, h * 0.6); + path.lineTo(w * 0.8, h * 0.6); + path.lineTo(w * 0.8, h); + path.lineTo(w * 0.2, h); + path.lineTo(w * 0.2, h * 0.6); + path.lineTo(0, h * 0.6); + break; + case ArrowDirection.down: + path.moveTo(w / 2, h); + path.lineTo(0, h * 0.4); + path.lineTo(w * 0.2, h * 0.4); + path.lineTo(w * 0.2, 0); + path.lineTo(w * 0.8, 0); + path.lineTo(w * 0.8, h * 0.4); + path.lineTo(w, h * 0.4); + break; + case ArrowDirection.left: + path.moveTo(0, h / 2); + path.lineTo(w * 0.6, 0); + path.lineTo(w * 0.6, h * 0.2); + path.lineTo(w, h * 0.2); + path.lineTo(w, h * 0.8); + path.lineTo(w * 0.6, h * 0.8); + path.lineTo(w * 0.6, h); + break; + case ArrowDirection.right: + path.moveTo(w, h / 2); + path.lineTo(w * 0.4, h); + path.lineTo(w * 0.4, h * 0.8); + path.lineTo(0, h * 0.8); + path.lineTo(0, h * 0.2); + path.lineTo(w * 0.4, h * 0.2); + path.lineTo(w * 0.4, 0); + break; + } + + path.close(); + canvas.drawPath(path, paint); + } + + @override + bool shouldRepaint(covariant _LinearArrowPainter oldDelegate) => false; +} + +class _SlantedArrowPad extends StatelessWidget { + final VoidCallback? onUp; + final VoidCallback? onDown; + final VoidCallback? onLeft; + final VoidCallback? onRight; + + const _SlantedArrowPad({ + this.onUp, + this.onDown, + this.onLeft, + this.onRight, + }); + + @override + Widget build(BuildContext context) { + return Center( + child: Transform( + transform: Matrix4.identity() + ..setEntry(3, 2, 0.0015) + ..rotateX(-0.9), + alignment: FractionalOffset.center, + child: SizedBox( + height: size, + width: size, + child: Stack( + children: [ + _BuildArrowButton(alignment: Alignment.topCenter, direction: ArrowDirection.up, onPressed: onUp, label: 'X-'), + _BuildArrowButton(alignment: Alignment.bottomCenter, direction: ArrowDirection.down, onPressed: onDown, label: 'X+'), + _BuildArrowButton(alignment: Alignment.centerLeft, direction: ArrowDirection.left, onPressed: onLeft, label: 'Y-'), + _BuildArrowButton(alignment: Alignment.centerRight, direction: ArrowDirection.right, onPressed: onRight, label: 'Y+'), + ], + ), + ), + ), + ); + } +} + +class _BuildArrowButton extends StatelessWidget { + final Alignment alignment; + final ArrowDirection direction; + final String label; + final VoidCallback? onPressed; + + const _BuildArrowButton({ + required this.alignment, + required this.direction, + required this.onPressed, + required this.label, + }); + + @override + Widget build(BuildContext context) { + return Align( + alignment: alignment, + child: SizedBox( + width: size / 2.5, + height: size / 2.5, + child: IconButton( + icon: Stack( + alignment: Alignment.center, + children: [ + CustomPaint( + painter: _LinearArrowPainter(direction: direction, color: Colors.black), + child: const SizedBox.expand(), + ), + Text( + label, + style: const TextStyle( + color: Colors.white, + fontWeight: FontWeight.bold, + fontSize: 16, + ), + ), + ], + ), + onPressed: onPressed, + ), + ), + ); + } +} diff --git a/example/viam_example_app/lib/resources/arm_widgets/orienation_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/orienation_widget.dart new file mode 100644 index 00000000000..a6213cb1f66 --- /dev/null +++ b/example/viam_example_app/lib/resources/arm_widgets/orienation_widget.dart @@ -0,0 +1,228 @@ +import 'package:flutter/material.dart'; +import 'package:flutter/services.dart'; +import 'package:viam_sdk/viam_sdk.dart'; + +class OrientationWidget extends StatefulWidget { + final Arm arm; + const OrientationWidget({super.key, required this.arm}); + + @override + State createState() => _ArmControlWidgetState(); +} + +class _ArmControlWidgetState extends State { + static const double _minOrientation = -1.0; + static const double _maxOrientation = 1.0; + final _controlCount = 4; + + static const double _minTheta = -180; + static const double _maxTheta = 180; + List _controlValues = []; + + late final List _textControllers; + + @override + void initState() { + super.initState(); + _getStartOrientation(); + } + + Future _getStartOrientation() async { + final startPose = await widget.arm.endPosition(); + _controlValues = [startPose.oX, startPose.oY, startPose.oZ, startPose.theta]; + _textControllers = List.generate( + _controlCount, + (index) => TextEditingController(text: _controlValues[index].toStringAsFixed(1)), + ); + setState(() {}); + } + + @override + void dispose() { + for (final controller in _textControllers) { + controller.dispose(); + } + super.dispose(); + } + + void _updateControlValue(int index, double value) { + final clampedValue = value.clamp(_minOrientation, _maxOrientation); + + setState(() { + _controlValues[index] = clampedValue; + + final formattedValue = clampedValue.toStringAsFixed(1); + if (_textControllers[index].text != formattedValue) { + _textControllers[index].text = formattedValue; + _textControllers[index].selection = TextSelection.fromPosition( + TextPosition(offset: _textControllers[index].text.length), + ); + } + }); + } + + void _updateThetaValue(int index, double value) { + final clampedValue = value.clamp(_minTheta, _maxTheta); + setState(() { + _controlValues[index] = clampedValue; + + final formattedValue = clampedValue.toStringAsFixed(1); + if (_textControllers[index].text != formattedValue) { + _textControllers[index].text = formattedValue; + _textControllers[index].selection = TextSelection.fromPosition( + TextPosition(offset: _textControllers[index].text.length), + ); + } + }); + } + + @override + Widget build(BuildContext context) { + return Column( + children: [ + Divider(), + Text( + 'End-effector Orientation', + style: TextStyle( + fontWeight: FontWeight.bold, + ), + ), + Divider(), + Padding( + padding: const EdgeInsets.all(16.0), + child: _controlValues.isEmpty + ? CircularProgressIndicator.adaptive() + : Column( + mainAxisSize: MainAxisSize.min, + children: [ + _BuildJointControlRow( + label: 'OX', + value: _controlValues[0], + controller: _textControllers[0], + min: _minOrientation, + max: _maxOrientation, + onValueChanged: (newValue) => _updateControlValue(0, newValue), + ), + _BuildJointControlRow( + label: 'OY', + value: _controlValues[1], + controller: _textControllers[1], + min: _minOrientation, + max: _maxOrientation, + onValueChanged: (newValue) => _updateControlValue(1, newValue), + ), + _BuildJointControlRow( + label: 'OZ', + value: _controlValues[2], + controller: _textControllers[2], + min: _minOrientation, + max: _maxOrientation, + onValueChanged: (newValue) => _updateControlValue(2, newValue), + ), + _BuildJointControlRow( + label: 'Theta', + value: _controlValues[3], + controller: _textControllers[3], + min: _minTheta, + max: _maxTheta, + onValueChanged: (newValue) => _updateThetaValue(3, newValue), + ), + ], + ), + ), + ], + ); + } +} + +class _BuildJointControlRow extends StatelessWidget { + final String label; + final double value; + final TextEditingController controller; + final double min; + final double max; + final ValueChanged onValueChanged; + + const _BuildJointControlRow({ + required this.label, + required this.value, + required this.controller, + required this.min, + required this.max, + required this.onValueChanged, + }); + + @override + Widget build(BuildContext context) { + return Padding( + padding: const EdgeInsets.symmetric(vertical: 8.0), + child: Row( + children: [ + // Control Label + SizedBox( + width: 70, + child: Text( + label, + style: Theme.of(context).textTheme.titleMedium, + ), + ), + Expanded( + child: SliderTheme( + data: SliderThemeData( + activeTrackColor: Colors.black, + inactiveTrackColor: Colors.grey, + thumbColor: Colors.black, + overlayColor: Colors.transparent, + showValueIndicator: ShowValueIndicator.never, + ), + child: Slider( + value: value, + min: min, + max: max, + divisions: ((max - min) * 10).toInt(), + label: value.toStringAsFixed(1), + onChanged: onValueChanged, + activeColor: Colors.black, + overlayColor: WidgetStateProperty.all(Colors.transparent), + ), + ), + ), + const SizedBox(width: 16), + SizedBox( + width: 70, + child: TextField( + controller: controller, + textAlign: TextAlign.center, + keyboardType: const TextInputType.numberWithOptions(decimal: true, signed: true), + inputFormatters: [ + FilteringTextInputFormatter.allow(RegExp(r'^-?\d+\.?\d{0,1}')), + ], + style: const TextStyle(color: Colors.black), + cursorColor: Colors.black, + decoration: const InputDecoration( + border: OutlineInputBorder(), + focusedBorder: OutlineInputBorder( + borderSide: BorderSide(color: Colors.black), + ), + contentPadding: EdgeInsets.symmetric(horizontal: 8), + ), + onSubmitted: (newValue) { + final parsedValue = double.tryParse(newValue) ?? value; + onValueChanged(parsedValue); + }, + ), + ), + const SizedBox(width: 8), + IconButton( + icon: const Icon(Icons.remove), + onPressed: () => onValueChanged(value - 0.1), + ), + IconButton( + icon: const Icon(Icons.add), + onPressed: () => onValueChanged(value + 0.1), + ), + ], + ), + ); + } +} diff --git a/example/viam_example_app/lib/resources/arm_widgets/position_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/position_widget.dart new file mode 100644 index 00000000000..10a1532df0d --- /dev/null +++ b/example/viam_example_app/lib/resources/arm_widgets/position_widget.dart @@ -0,0 +1,203 @@ +import 'package:flutter/material.dart'; +import 'package:flutter/services.dart'; +import 'package:viam_sdk/viam_sdk.dart'; + +class PositionWidget extends StatefulWidget { + final Arm arm; + const PositionWidget({super.key, required this.arm}); + + @override + State createState() => _ArmControlWidgetState(); +} + +class _ArmControlWidgetState extends State { + static const double _minPosition = -10000; + static const double _maxPosition = 10000; + final _controlCount = 3; + + List _controlValues = []; + + late final List _textControllers; + + @override + void initState() { + super.initState(); + _getStartOrientation(); + } + + Future _getStartOrientation() async { + final startPose = await widget.arm.endPosition(); + _controlValues = [startPose.x, startPose.y, startPose.z]; + _textControllers = List.generate( + _controlCount, + (index) => TextEditingController(text: _controlValues[index].toStringAsFixed(1)), + ); + setState(() {}); + } + + @override + void dispose() { + for (final controller in _textControllers) { + controller.dispose(); + } + super.dispose(); + } + + void _updateControlValue(int index, double value) { + final clampedValue = value.clamp(_minPosition, _maxPosition); + + setState(() { + _controlValues[index] = clampedValue; + + final formattedValue = clampedValue.toStringAsFixed(1); + if (_textControllers[index].text != formattedValue) { + _textControllers[index].text = formattedValue; + _textControllers[index].selection = TextSelection.fromPosition( + TextPosition(offset: _textControllers[index].text.length), + ); + } + }); + } + + @override + Widget build(BuildContext context) { + return Column( + children: [ + Divider(), + Text( + 'End-effector Position', + style: TextStyle( + fontWeight: FontWeight.bold, + ), + ), + Divider(), + Padding( + padding: const EdgeInsets.all(16.0), + child: _controlValues.isEmpty + ? CircularProgressIndicator.adaptive() + : Column( + mainAxisSize: MainAxisSize.min, + children: [ + _BuildJointControlRow( + label: 'X', + value: _controlValues[0], + controller: _textControllers[0], + min: _minPosition, + max: _maxPosition, + onValueChanged: (newValue) => _updateControlValue(0, newValue), + ), + _BuildJointControlRow( + label: 'Y', + value: _controlValues[1], + controller: _textControllers[1], + min: _minPosition, + max: _maxPosition, + onValueChanged: (newValue) => _updateControlValue(1, newValue), + ), + _BuildJointControlRow( + label: 'Z', + value: _controlValues[2], + controller: _textControllers[2], + min: _minPosition, + max: _maxPosition, + onValueChanged: (newValue) => _updateControlValue(2, newValue), + ), + ], + ), + ), + ], + ); + } +} + +class _BuildJointControlRow extends StatelessWidget { + final String label; + final double value; + final TextEditingController controller; + final double min; + final double max; + final ValueChanged onValueChanged; + + const _BuildJointControlRow({ + required this.label, + required this.value, + required this.controller, + required this.min, + required this.max, + required this.onValueChanged, + }); + + @override + Widget build(BuildContext context) { + return Padding( + padding: const EdgeInsets.symmetric(vertical: 8.0), + child: Row( + children: [ + // Control Label + SizedBox( + width: 70, + child: Text( + label, + style: Theme.of(context).textTheme.titleMedium, + ), + ), + Expanded( + child: SliderTheme( + data: SliderThemeData( + activeTrackColor: Colors.black, + inactiveTrackColor: Colors.grey, + thumbColor: Colors.black, + overlayColor: Colors.transparent, + showValueIndicator: ShowValueIndicator.never, + ), + child: Slider( + value: value, + min: min, + max: max, + divisions: ((max - min) * 10).toInt(), + label: value.toStringAsFixed(1), + onChanged: onValueChanged, + activeColor: Colors.black, + overlayColor: WidgetStateProperty.all(Colors.transparent), + ), + ), + ), + const SizedBox(width: 16), + SizedBox( + width: 70, + child: TextField( + controller: controller, + textAlign: TextAlign.center, + keyboardType: const TextInputType.numberWithOptions(decimal: true, signed: true), + inputFormatters: [ + FilteringTextInputFormatter.allow(RegExp(r'^-?\d+\.?\d{0,1}')), + ], + style: const TextStyle(color: Colors.black), + cursorColor: Colors.black, + decoration: const InputDecoration( + border: OutlineInputBorder(), + focusedBorder: OutlineInputBorder( + borderSide: BorderSide(color: Colors.black), + ), + contentPadding: EdgeInsets.symmetric(horizontal: 8), + ), + onSubmitted: (newValue) { + final parsedValue = double.tryParse(newValue) ?? value; + onValueChanged(parsedValue); + }, + ), + ), + const SizedBox(width: 8), + IconButton( + icon: const Icon(Icons.remove), + onPressed: () => onValueChanged(value - 0.1), + ), + IconButton( + icon: const Icon(Icons.add), + onPressed: () => onValueChanged(value + 0.1), + ), + ], + ), + ); + } +} diff --git a/example/viam_example_app/lib/robot_screen.dart b/example/viam_example_app/lib/robot_screen.dart index f533a565186..1f8025599bc 100644 --- a/example/viam_example_app/lib/robot_screen.dart +++ b/example/viam_example_app/lib/robot_screen.dart @@ -5,6 +5,7 @@ /// and send commands to them. import 'package:flutter/material.dart'; +import 'package:viam_example_app/resources/arm_screen.dart'; import 'package:viam_sdk/protos/app/app.dart'; import 'package:viam_sdk/viam_sdk.dart'; @@ -81,6 +82,7 @@ class _RobotScreenState extends State { final availableResourceSubtypes = [ Camera.subtype.resourceSubtype, Motor.subtype.resourceSubtype, + Arm.subtype.resourceSubtype, ]; return availableResourceSubtypes.contains(rn.subtype); } @@ -116,27 +118,52 @@ class _RobotScreenState extends State { // Similar to camera above, get the motor from the robot client. final motor = Motor.fromRobot(client, rn.name); Navigator.of(context).push(MaterialPageRoute(builder: (_) => MotorScreen(motor))); + } else if (rn.subtype == Arm.subtype.resourceSubtype) { + final arm = Arm.fromRobot(client, rn.name); + Navigator.of(context).push(MaterialPageRoute(builder: (_) => ViamArmWidgetNew(arm: arm))); } } + Widget getResourceWidget(ResourceName rName) { + if (rName.subtype == Arm.subtype.resourceSubtype) { + return Padding(padding: EdgeInsets.all(4), child: ViamArmWidgetNew(arm: Arm.fromRobot(client, rName.name))); + } + return const Text( + 'No screen selected!', + ); + } + @override Widget build(BuildContext context) { return Scaffold( - appBar: AppBar(title: Text(widget.robot.name)), - body: _isLoading - ? const Center(child: CircularProgressIndicator.adaptive()) - : ListView.builder( - itemCount: client.resourceNames.length, - itemBuilder: (_, index) { - final resourceName = _sortedResourceNames[index]; - return ListTile( - title: Text(resourceName.name), - subtitle: Text('${resourceName.namespace}:${resourceName.type}:${resourceName.subtype}'), - // We only want to navigate to a resource if that resource is one that we implemented - onTap: _isNavigable(resourceName) ? () => _navigateToResource(resourceName) : null, - // Similarly, we only want to show the navigation icon if the resource is implemented - trailing: _isNavigable(resourceName) ? const Icon(Icons.chevron_right) : null, - ); - })); + appBar: AppBar( + title: Text(widget.robot.name), + ), + body: SingleChildScrollView( + child: Column( + children: [ + for (int i = 0; i < _sortedResourceNames.length; i++) + _isNavigable(_sortedResourceNames[i]) + ? ExpansionTile( + title: Text(_sortedResourceNames[i].name), + subtitle: + Text('${_sortedResourceNames[i].namespace}:${_sortedResourceNames[i].type}:${_sortedResourceNames[i].subtype}'), + children: [ + Container( + color: Theme.of(context).colorScheme.surface, + child: getResourceWidget(_sortedResourceNames[i]), + ) + ], + ) + : ListTile( + title: Text(_sortedResourceNames[i].name), + subtitle: + Text('${_sortedResourceNames[i].namespace}:${_sortedResourceNames[i].type}:${_sortedResourceNames[i].subtype}'), + enabled: false, + ) + ], + ), + ), + ); } } From a041430684daaf8a8e9f2d1204c23493f3abbde8 Mon Sep 17 00:00:00 2001 From: martha-johnston Date: Wed, 22 Oct 2025 10:27:53 -0400 Subject: [PATCH 02/27] prototype --- example/viam_example_app/lib/resources/arm_screen.dart | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_screen.dart b/example/viam_example_app/lib/resources/arm_screen.dart index a05c1d3d9dd..5348395e6a4 100644 --- a/example/viam_example_app/lib/resources/arm_screen.dart +++ b/example/viam_example_app/lib/resources/arm_screen.dart @@ -1,6 +1,5 @@ import 'package:flutter/material.dart'; import 'package:viam_example_app/resources/arm_widgets/joint_positions_widget.dart'; -import 'package:viam_example_app/resources/arm_widgets/linear_arrows_widget.dart'; import 'package:viam_example_app/resources/arm_widgets/orienation_widget.dart'; import 'package:viam_example_app/resources/arm_widgets/position_widget.dart'; import 'package:viam_sdk/viam_sdk.dart'; @@ -20,7 +19,7 @@ class ViamArmWidgetNew extends StatelessWidget { return Column( children: [ JointPositionsWidget(arm: arm), - LinearArrowsWidget(), + // LinearArrowsWidget(), PositionWidget(arm: arm), // AngularArrowsWidget(), // JointPositionsWidget(arm: arm), From 7197ceb4fabac1a5e01808d1a1bf2e0f455ba186 Mon Sep 17 00:00:00 2001 From: martha-johnston Date: Wed, 22 Oct 2025 13:24:46 -0400 Subject: [PATCH 03/27] add live toggle --- .../arm_widgets/joint_positions_widget.dart | 46 ++++++++++++++++-- .../arm_widgets/orienation_widget.dart | 42 ++++++++++++++++- .../arm_widgets/position_widget.dart | 47 +++++++++++++++++-- 3 files changed, 124 insertions(+), 11 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/joint_positions_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/joint_positions_widget.dart index 5e83882555d..058a90d8442 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/joint_positions_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/joint_positions_widget.dart @@ -1,9 +1,9 @@ import 'package:flutter/material.dart'; import 'package:flutter/services.dart'; -import 'package:viam_sdk/viam_sdk.dart'; +import 'package:viam_sdk/viam_sdk.dart' as viam; class JointPositionsWidget extends StatefulWidget { - final Arm arm; + final viam.Arm arm; const JointPositionsWidget({super.key, required this.arm}); @override @@ -12,6 +12,7 @@ class JointPositionsWidget extends StatefulWidget { class _JointPositionsWidgetState extends State { List _startJointValues = []; + bool _isLive = false; @override void initState() { @@ -40,7 +41,7 @@ class _JointPositionsWidgetState extends State { ), Divider(), Padding( - padding: const EdgeInsets.all(16.0), + padding: const EdgeInsets.all(20.0), child: Column( mainAxisSize: MainAxisSize.min, children: _startJointValues.isEmpty @@ -50,6 +51,43 @@ class _JointPositionsWidgetState extends State { }), ), ), + Padding( + padding: const EdgeInsets.fromLTRB(20.0, 0, 20.0, 20.0), + child: Row( + spacing: 8, + children: [ + Switch( + value: _isLive, + activeColor: Colors.green, + inactiveTrackColor: Colors.transparent, + onChanged: (newValue) { + setState(() { + _isLive = newValue; + }); + }, + ), + Text( + "Live", + style: TextStyle(color: Colors.black), + ), + Spacer(), + OutlinedButtonTheme( + data: OutlinedButtonThemeData( + style: OutlinedButton.styleFrom( + foregroundColor: Colors.black, + iconColor: Colors.black, + overlayColor: Colors.grey, + shape: RoundedRectangleBorder(borderRadius: BorderRadius.all(Radius.circular(4)))), + ), + child: OutlinedButton.icon( + onPressed: _isLive ? null : () {}, + label: Text("Execute"), + icon: Icon(Icons.play_arrow), + ), + ) + ], + ), + ), ], ); } @@ -57,7 +95,7 @@ class _JointPositionsWidgetState extends State { class _BuildJointControlRow extends StatefulWidget { final int index; - final Arm arm; + final viam.Arm arm; final List startJointValues; const _BuildJointControlRow({required this.index, required this.arm, required this.startJointValues}); diff --git a/example/viam_example_app/lib/resources/arm_widgets/orienation_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/orienation_widget.dart index a6213cb1f66..002c30280de 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/orienation_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/orienation_widget.dart @@ -1,9 +1,9 @@ import 'package:flutter/material.dart'; import 'package:flutter/services.dart'; -import 'package:viam_sdk/viam_sdk.dart'; +import 'package:viam_sdk/viam_sdk.dart' as viam; class OrientationWidget extends StatefulWidget { - final Arm arm; + final viam.Arm arm; const OrientationWidget({super.key, required this.arm}); @override @@ -14,6 +14,7 @@ class _ArmControlWidgetState extends State { static const double _minOrientation = -1.0; static const double _maxOrientation = 1.0; final _controlCount = 4; + bool _isLive = false; static const double _minTheta = -180; static const double _maxTheta = 180; @@ -130,6 +131,43 @@ class _ArmControlWidgetState extends State { ], ), ), + Padding( + padding: const EdgeInsets.fromLTRB(20.0, 0, 20.0, 20.0), + child: Row( + spacing: 8, + children: [ + Switch( + value: _isLive, + activeColor: Colors.green, + inactiveTrackColor: Colors.transparent, + onChanged: (newValue) { + setState(() { + _isLive = newValue; + }); + }, + ), + Text( + "Live", + style: TextStyle(color: Colors.black), + ), + Spacer(), + OutlinedButtonTheme( + data: OutlinedButtonThemeData( + style: OutlinedButton.styleFrom( + foregroundColor: Colors.black, + iconColor: Colors.black, + overlayColor: Colors.grey, + shape: RoundedRectangleBorder(borderRadius: BorderRadius.all(Radius.circular(4)))), + ), + child: OutlinedButton.icon( + onPressed: _isLive ? null : () {}, + label: Text("Execute"), + icon: Icon(Icons.play_arrow), + ), + ) + ], + ), + ), ], ); } diff --git a/example/viam_example_app/lib/resources/arm_widgets/position_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/position_widget.dart index 10a1532df0d..a67e925b419 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/position_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/position_widget.dart @@ -1,9 +1,9 @@ import 'package:flutter/material.dart'; import 'package:flutter/services.dart'; -import 'package:viam_sdk/viam_sdk.dart'; +import 'package:viam_sdk/viam_sdk.dart' as viam; class PositionWidget extends StatefulWidget { - final Arm arm; + final viam.Arm arm; const PositionWidget({super.key, required this.arm}); @override @@ -14,6 +14,7 @@ class _ArmControlWidgetState extends State { static const double _minPosition = -10000; static const double _maxPosition = 10000; final _controlCount = 3; + bool _isLive = false; List _controlValues = []; @@ -72,7 +73,7 @@ class _ArmControlWidgetState extends State { ), Divider(), Padding( - padding: const EdgeInsets.all(16.0), + padding: const EdgeInsets.all(20.0), child: _controlValues.isEmpty ? CircularProgressIndicator.adaptive() : Column( @@ -105,6 +106,43 @@ class _ArmControlWidgetState extends State { ], ), ), + Padding( + padding: const EdgeInsets.fromLTRB(20.0, 0, 20.0, 20.0), + child: Row( + spacing: 8, + children: [ + Switch( + value: _isLive, + activeColor: Colors.green, + inactiveTrackColor: Colors.transparent, + onChanged: (newValue) { + setState(() { + _isLive = newValue; + }); + }, + ), + Text( + "Live", + style: TextStyle(color: Colors.black), + ), + Spacer(), + OutlinedButtonTheme( + data: OutlinedButtonThemeData( + style: OutlinedButton.styleFrom( + foregroundColor: Colors.black, + iconColor: Colors.black, + overlayColor: Colors.grey, + shape: RoundedRectangleBorder(borderRadius: BorderRadius.all(Radius.circular(4)))), + ), + child: OutlinedButton.icon( + onPressed: _isLive ? null : () {}, + label: Text("Execute"), + icon: Icon(Icons.play_arrow), + ), + ) + ], + ), + ), ], ); } @@ -133,9 +171,8 @@ class _BuildJointControlRow extends StatelessWidget { padding: const EdgeInsets.symmetric(vertical: 8.0), child: Row( children: [ - // Control Label SizedBox( - width: 70, + width: 30, child: Text( label, style: Theme.of(context).textTheme.titleMedium, From 5df7ea3d25a7f42fb00b43606851b9cd550f6af3 Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Tue, 4 Nov 2025 16:56:52 -0500 Subject: [PATCH 04/27] imu prototype --- .../lib/resources/arm_screen.dart | 11 + .../lib/resources/arm_widgets/imu_widget.dart | 333 ++++++++++++++++++ .../viam_example_app/lib/robot_screen.dart | 75 ++-- example/viam_example_app/pubspec.yaml | 1 + example/viam_robot_example_app/pubspec.yaml | 2 + pubspec.yaml | 2 +- 6 files changed, 389 insertions(+), 35 deletions(-) create mode 100644 example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart diff --git a/example/viam_example_app/lib/resources/arm_screen.dart b/example/viam_example_app/lib/resources/arm_screen.dart index 5348395e6a4..2b16784e218 100644 --- a/example/viam_example_app/lib/resources/arm_screen.dart +++ b/example/viam_example_app/lib/resources/arm_screen.dart @@ -1,4 +1,5 @@ import 'package:flutter/material.dart'; +import 'package:viam_example_app/resources/arm_widgets/imu_widget.dart'; import 'package:viam_example_app/resources/arm_widgets/joint_positions_widget.dart'; import 'package:viam_example_app/resources/arm_widgets/orienation_widget.dart'; import 'package:viam_example_app/resources/arm_widgets/position_widget.dart'; @@ -24,7 +25,17 @@ class ViamArmWidgetNew extends StatelessWidget { // AngularArrowsWidget(), // JointPositionsWidget(arm: arm), OrientationWidget(arm: arm), + ImuWidget(arm: arm, updateNotifier: ArmNotifier()), ], ); } } + +// need real arm notifier from martha +class ArmNotifier extends ChangeNotifier { + ArmNotifier(); + + void update() { + notifyListeners(); + } +} \ No newline at end of file diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart new file mode 100644 index 00000000000..17f1e4cf1c6 --- /dev/null +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -0,0 +1,333 @@ +import 'dart:async'; + +import 'package:flutter/material.dart'; +import 'package:sensors_plus/sensors_plus.dart'; +import 'package:viam_example_app/resources/arm_screen.dart'; +import 'package:viam_sdk/viam_sdk.dart'; + +class ImuWidget extends StatefulWidget { + final Arm arm; + final ArmNotifier updateNotifier; + const ImuWidget({ + super.key, + required this.arm, + required this.updateNotifier, + }); + @override + State createState() => _ImuWidgetState(); +} + +class _ImuWidgetState extends State { + @override + void initState() { + _initAccelerometer(); + super.initState(); + } + + final _streamSubscriptions = >[]; + Duration sensorInterval = SensorInterval.normalInterval; + + // Arm movement control variables + DateTime? _lastArmCommandTime; + static const Duration _armCommandInterval = Duration(milliseconds: 100); // Rate limit: 10 commands/sec + static const double _positionScale = 50.0; // Scale factor: mm of arm movement per meter of phone movement + static const double _velocityDecay = 0.95; // Decay factor to prevent drift + static const double _deadZone = 0.5; // Ignore small accelerations + bool _isMovingArm = false; + String? _lastError; + + // Position tracking via integration + double _velocityX = 0.0; + double _velocityY = 0.0; + double _velocityZ = 0.0; + double _positionX = 0.0; // Accumulated phone displacement in meters from reference + double _positionY = 0.0; + double _positionZ = 0.0; + DateTime? _lastIntegrationTime; + + // Orientation tracking via gyroscope integration + double _orientationX = 0.0; // Roll (rotation around X-axis, in radians) + double _orientationY = 0.0; // Pitch (rotation around Y-axis, in radians) + double _orientationZ = 0.0; // Yaw (rotation around Z-axis, in radians) + DateTime? _lastGyroIntegrationTime; + + // Reference positions (set when "Set Reference" is pressed) + Pose? _referenceArmPose; // Arm position when reference was set + bool _isReferenceSet = false; + + // Current arm position (for display) + Pose? _currentArmPose; + + _initAccelerometer() { + _streamSubscriptions.add( + userAccelerometerEventStream(samplingPeriod: sensorInterval).listen( + (UserAccelerometerEvent event) { + // Move arm based on accelerometer data + _moveArmFromImu(event); + }, + onError: (e) { + showDialog( + context: context, + builder: (context) { + return const AlertDialog( + title: Text("Sensor Not Found"), + content: Text("It seems that your device doesn't support User Accelerometer Sensor"), + ); + }); + }, + cancelOnError: true, + ), + ); + + _streamSubscriptions.add( + gyroscopeEventStream(samplingPeriod: sensorInterval).listen( + (GyroscopeEvent event) { + // Update orientation based on gyroscope data + _updateOrientationFromGyroscope(event); + }, + onError: (e) { + showDialog( + context: context, + builder: (context) { + return const AlertDialog( + title: Text("Sensor Not Found"), + content: Text("It seems that your device doesn't support Gyroscope Sensor"), + ); + }); + }, + cancelOnError: true, + ), + ); + } + + /// Update orientation by integrating gyroscope angular velocity + void _updateOrientationFromGyroscope(GyroscopeEvent event) { + final now = DateTime.now(); + + // Initialize integration time on first run + if (_lastGyroIntegrationTime == null) { + _lastGyroIntegrationTime = now; + return; + } + + // Calculate time delta for integration (in seconds) + final dt = now.difference(_lastGyroIntegrationTime!).inMilliseconds / 1000.0; + _lastGyroIntegrationTime = now; + + // Skip if dt is too large (indicates pause or first run) + if (dt > 0.5) { + return; + } + + // Don't update orientation if reference point hasn't been set + if (!_isReferenceSet) { + return; + } + + // Integrate angular velocity to get orientation change + // Gyroscope values are in radians/second + // event.x = rotation rate around X-axis (roll) + // event.y = rotation rate around Y-axis (pitch) + // event.z = rotation rate around Z-axis (yaw) + _orientationX += event.x * dt; + _orientationY += event.y * dt; + _orientationZ += event.z * dt; + + // Optional: Apply small decay to prevent drift + _orientationX *= 0.999; + _orientationY *= 0.999; + _orientationZ *= 0.999; + } + + /// Move the arm based on IMU accelerometer data by integrating acceleration to position + Future _moveArmFromImu(UserAccelerometerEvent event) async { + final now = DateTime.now(); + + // Initialize integration time on first run + if (_lastIntegrationTime == null) { + _lastIntegrationTime = now; + return; + } + + // Calculate time delta for integration (in seconds) + final dt = now.difference(_lastIntegrationTime!).inMilliseconds / 1000.0; + _lastIntegrationTime = now; + + // Skip if dt is too large (indicates pause or first run) + if (dt > 0.5) { + return; + } + + // Apply dead zone to acceleration + final accelX = event.x.abs() > _deadZone ? event.x : 0.0; + final accelY = event.y.abs() > _deadZone ? event.y : 0.0; + final accelZ = event.z.abs() > _deadZone ? event.z : 0.0; + + // STEP 1: Integrate acceleration to get velocity (v = v0 + a*dt) + _velocityX += accelX * dt; + _velocityY += accelY * dt; + _velocityZ += accelZ * dt; + + // Apply decay to prevent drift when stationary + _velocityX *= _velocityDecay; + _velocityY *= _velocityDecay; + _velocityZ *= _velocityDecay; + + // STEP 2: Integrate velocity to get position (p = p0 + v*dt) + _positionX += _velocityX * dt; + _positionY += _velocityY * dt; + _positionZ += _velocityZ * dt; + + // Rate limiting: don't send arm commands too frequently + if (_lastArmCommandTime != null) { + final timeSinceLastCommand = now.difference(_lastArmCommandTime!); + if (timeSinceLastCommand < _armCommandInterval) { + return; // Too soon, skip this update + } + } + + // Don't send new command if previous one is still executing + if (_isMovingArm) { + return; + } + + // Don't move arm if reference point hasn't been set + if (!_isReferenceSet || _referenceArmPose == null) { + return; + } + + _lastArmCommandTime = now; + _isMovingArm = true; + + try { + // 3. Calculate new target position based on reference + phone displacement + // Reference arm position + (phone displacement * scale factor) + // Phone displacement is in meters, arm position is in mm, so scale appropriately + final newX = _referenceArmPose!.x + (_positionX * _positionScale); + final newY = _referenceArmPose!.y + (_positionY * _positionScale); + final newZ = _referenceArmPose!.z + (_positionZ * _positionScale); + + // 4. Create new pose with orientation from gyroscope + // Combine reference orientation with the accumulated orientation changes + final newPose = Pose( + x: newX, + y: newY, + z: newZ, + theta: _referenceArmPose!.theta, // Keep theta from reference + oX: _referenceArmPose!.oX + _orientationX, + oY: _referenceArmPose!.oY + _orientationY, + oZ: _referenceArmPose!.oZ + _orientationZ, + ); + + // 5. Move the arm to the new position + await widget.arm.moveToPosition(newPose); + + setState(() { + _lastError = null; + _currentArmPose = newPose; // Store for display + }); + } catch (e) { + // Handle errors gracefully + setState(() { + _lastError = e.toString(); + }); + } finally { + _isMovingArm = false; + } + } + + /// Set reference point: links current phone position to current arm position + Future _setReference() async { + try { + // Get the current arm position + final currentArmPose = await widget.arm.endPosition(); + + setState(() { + // Zero out the phone position tracking (this is the new starting point) + _positionX = 0.0; + _positionY = 0.0; + _positionZ = 0.0; + _velocityX = 0.0; + _velocityY = 0.0; + _velocityZ = 0.0; + _lastIntegrationTime = null; + + // Zero out orientation tracking + _orientationX = 0.0; + _orientationY = 0.0; + _orientationZ = 0.0; + _lastGyroIntegrationTime = null; + + // Store the current arm position as the reference + _referenceArmPose = currentArmPose; + _currentArmPose = currentArmPose; // Also store for display + _isReferenceSet = true; + _lastError = null; + }); + } catch (e) { + setState(() { + _lastError = "Failed to set reference: ${e.toString()}"; + }); + } + } + + @override + Widget build(BuildContext context) { + return Column( + mainAxisAlignment: MainAxisAlignment.center, + children: [ + const Text("Arm Position (Real World)", style: TextStyle(fontWeight: FontWeight.bold, fontSize: 18)), + const SizedBox(height: 20), + if (_currentArmPose != null) ...[ + Text("X: ${_currentArmPose!.x.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 16)), + Text("Y: ${_currentArmPose!.y.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 16)), + Text("Z: ${_currentArmPose!.z.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 16)), + const SizedBox(height: 15), + const Text("Orientation:", style: TextStyle(fontWeight: FontWeight.bold, fontSize: 16)), + Text("oX (Roll): ${_currentArmPose!.oX.toStringAsFixed(3)} rad", style: const TextStyle(fontSize: 14)), + Text("oY (Pitch): ${_currentArmPose!.oY.toStringAsFixed(3)} rad", style: const TextStyle(fontSize: 14)), + Text("oZ (Yaw): ${_currentArmPose!.oZ.toStringAsFixed(3)} rad", style: const TextStyle(fontSize: 14)), + ] else + const Text("No position data yet", style: TextStyle(fontSize: 14, color: Colors.grey)), + const SizedBox(height: 30), + ElevatedButton( + onPressed: _setReference, + style: ElevatedButton.styleFrom( + backgroundColor: _isReferenceSet ? Colors.green : Colors.blue, + ), + child: Text(_isReferenceSet ? "Reset Reference" : "Set Reference Point"), + ), + const SizedBox(height: 15), + Text("Status: ${!_isReferenceSet ? 'Waiting for reference...' : _isMovingArm ? 'Moving...' : 'Ready'}"), + if (_lastError != null) + Padding( + padding: const EdgeInsets.all(8.0), + child: Text( + "Error: $_lastError", + style: const TextStyle(color: Colors.red, fontSize: 10), + textAlign: TextAlign.center, + ), + ), + const SizedBox(height: 15), + if (!_isReferenceSet) + const Text( + "Press 'Set Reference Point' to begin", + style: TextStyle(fontSize: 12, fontStyle: FontStyle.italic, color: Colors.orange), + ) + else + const Text( + "Move your phone through space!", + style: TextStyle(fontSize: 12, fontStyle: FontStyle.italic), + ), + ], + ); + } + + @override + void dispose() { + super.dispose(); + for (final subscription in _streamSubscriptions) { + subscription.cancel(); + } + } +} diff --git a/example/viam_example_app/lib/robot_screen.dart b/example/viam_example_app/lib/robot_screen.dart index 1f8025599bc..3a0df80be7d 100644 --- a/example/viam_example_app/lib/robot_screen.dart +++ b/example/viam_example_app/lib/robot_screen.dart @@ -5,6 +5,7 @@ /// and send commands to them. import 'package:flutter/material.dart'; +import 'package:flutter_dotenv/flutter_dotenv.dart'; import 'package:viam_example_app/resources/arm_screen.dart'; import 'package:viam_sdk/protos/app/app.dart'; import 'package:viam_sdk/viam_sdk.dart'; @@ -35,7 +36,7 @@ class _RobotScreenState extends State { /// /// This is initialized late because it requires an asynchronous /// network call to establish the connection. - late RobotClient client; + RobotClient? client; @override void initState() { @@ -49,7 +50,7 @@ class _RobotScreenState extends State { // You should always close the [RobotClient] to free up resources. // Calling [RobotClient.close] will clean up any tasks and // resources created by Viam. - client.close(); + client?.close(); super.dispose(); } @@ -59,7 +60,10 @@ class _RobotScreenState extends State { // Using the authenticated [Viam] the received as a parameter, // we can obtain a connection to the Robot. // There is a helpful convenience method on the [Viam] instance for this. - final robotClient = await widget._viam.getRobotClient(widget.robot); + // final robotClient = await widget._viam.getRobotClient(widget.robot); + final options = RobotClientOptions.withApiKey(dotenv.env['API_KEY_ID']!, dotenv.env['API_KEY']!); + options.dialOptions.attemptMdns = false; + final robotClient = await RobotClient.atAddress(dotenv.env['ROBOT_LOCATION']!, options); setState(() { client = robotClient; _isLoading = false; @@ -69,7 +73,8 @@ class _RobotScreenState extends State { /// A computed variable that returns the available [ResourceName]s of /// this robot in an alphabetically sorted list. List get _sortedResourceNames { - return client.resourceNames..sort((a, b) => a.name.compareTo(b.name)); + return client?.resourceNames ?? [] + ..sort((a, b) => a.name.compareTo(b.name)); } /// For this example, we have control screens for only these specific resource subtypes: @@ -106,27 +111,27 @@ class _RobotScreenState extends State { // [RobotClient.getResource(ResourceName)] // to get a resource directly from a [RobotClient]. // e.g. client.getResource(rn) - final camera = Camera.fromRobot(client, rn.name); + final camera = Camera.fromRobot(client!, rn.name); // A [StreamClient] is a WebRTC stream that allows you to view // a live stream from the camera. This requires that the connection // to the smart machine be through WebRTC (the default option). // If the connection is not using WebRTC, then this will error. - final stream = client.getStream(rn.name); + final stream = client!.getStream(rn.name); Navigator.of(context).push(MaterialPageRoute(builder: (_) => CameraScreen(camera, stream))); } else if (rn.subtype == Motor.subtype.resourceSubtype) { // Similar to camera above, get the motor from the robot client. - final motor = Motor.fromRobot(client, rn.name); + final motor = Motor.fromRobot(client!, rn.name); Navigator.of(context).push(MaterialPageRoute(builder: (_) => MotorScreen(motor))); } else if (rn.subtype == Arm.subtype.resourceSubtype) { - final arm = Arm.fromRobot(client, rn.name); + final arm = Arm.fromRobot(client!, rn.name); Navigator.of(context).push(MaterialPageRoute(builder: (_) => ViamArmWidgetNew(arm: arm))); } } Widget getResourceWidget(ResourceName rName) { if (rName.subtype == Arm.subtype.resourceSubtype) { - return Padding(padding: EdgeInsets.all(4), child: ViamArmWidgetNew(arm: Arm.fromRobot(client, rName.name))); + return Padding(padding: EdgeInsets.all(4), child: ViamArmWidgetNew(arm: Arm.fromRobot(client!, rName.name))); } return const Text( 'No screen selected!', @@ -139,31 +144,33 @@ class _RobotScreenState extends State { appBar: AppBar( title: Text(widget.robot.name), ), - body: SingleChildScrollView( - child: Column( - children: [ - for (int i = 0; i < _sortedResourceNames.length; i++) - _isNavigable(_sortedResourceNames[i]) - ? ExpansionTile( - title: Text(_sortedResourceNames[i].name), - subtitle: - Text('${_sortedResourceNames[i].namespace}:${_sortedResourceNames[i].type}:${_sortedResourceNames[i].subtype}'), - children: [ - Container( - color: Theme.of(context).colorScheme.surface, - child: getResourceWidget(_sortedResourceNames[i]), - ) - ], - ) - : ListTile( - title: Text(_sortedResourceNames[i].name), - subtitle: - Text('${_sortedResourceNames[i].namespace}:${_sortedResourceNames[i].type}:${_sortedResourceNames[i].subtype}'), - enabled: false, - ) - ], - ), - ), + body: _isLoading + ? const Center(child: CircularProgressIndicator()) + : SingleChildScrollView( + child: Column( + children: [ + for (int i = 0; i < _sortedResourceNames.length; i++) + _isNavigable(_sortedResourceNames[i]) + ? ExpansionTile( + title: Text(_sortedResourceNames[i].name), + subtitle: Text( + '${_sortedResourceNames[i].namespace}:${_sortedResourceNames[i].type}:${_sortedResourceNames[i].subtype}'), + children: [ + Container( + color: Theme.of(context).colorScheme.surface, + child: getResourceWidget(_sortedResourceNames[i]), + ) + ], + ) + : ListTile( + title: Text(_sortedResourceNames[i].name), + subtitle: Text( + '${_sortedResourceNames[i].namespace}:${_sortedResourceNames[i].type}:${_sortedResourceNames[i].subtype}'), + enabled: false, + ) + ], + ), + ), ); } } diff --git a/example/viam_example_app/pubspec.yaml b/example/viam_example_app/pubspec.yaml index 5f36cb72807..bb3ad2ca564 100644 --- a/example/viam_example_app/pubspec.yaml +++ b/example/viam_example_app/pubspec.yaml @@ -10,6 +10,7 @@ dependencies: flutter: sdk: flutter flutter_dotenv: ^5.1.0 + sensors_plus: ^7.0.0 viam_sdk: path: ../../ diff --git a/example/viam_robot_example_app/pubspec.yaml b/example/viam_robot_example_app/pubspec.yaml index e7ceff821f5..402228f2f99 100644 --- a/example/viam_robot_example_app/pubspec.yaml +++ b/example/viam_robot_example_app/pubspec.yaml @@ -14,6 +14,8 @@ dependencies: path: ../../ image: ^4.0.17 flutter_dotenv: ^5.1.0 + sensors_plus: ^7.0.0 + dev_dependencies: flutter_test: diff --git a/pubspec.yaml b/pubspec.yaml index 6316cebb68c..fee09df03af 100644 --- a/pubspec.yaml +++ b/pubspec.yaml @@ -11,7 +11,7 @@ dependencies: flutter: sdk: flutter flutter_webrtc: ^0.12.1+hotfix.1 - grpc: ^4.0.1 + grpc: ^4.1.0 protobuf: ^3.0.0 image: ^4.0.16 logger: ^2.0.1 From 0180f92d3380cedeffed0c420d12b49e1433ca47 Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Wed, 5 Nov 2025 08:20:38 -0500 Subject: [PATCH 05/27] update comments for better understanding --- .../lib/resources/arm_widgets/imu_widget.dart | 137 ++++++++++-------- 1 file changed, 79 insertions(+), 58 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index 17f1e4cf1c6..c4246e5af0f 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -30,32 +30,35 @@ class _ImuWidgetState extends State { // Arm movement control variables DateTime? _lastArmCommandTime; static const Duration _armCommandInterval = Duration(milliseconds: 100); // Rate limit: 10 commands/sec - static const double _positionScale = 50.0; // Scale factor: mm of arm movement per meter of phone movement + + // Scale factor: 50mm/meter = 50mm of arm movement per meter of phone movement + static const double _positionScale = 50.0; + static const double _velocityDecay = 0.95; // Decay factor to prevent drift static const double _deadZone = 0.5; // Ignore small accelerations bool _isMovingArm = false; String? _lastError; - // Position tracking via integration + // Velocity (meters per second) double _velocityX = 0.0; double _velocityY = 0.0; double _velocityZ = 0.0; - double _positionX = 0.0; // Accumulated phone displacement in meters from reference + // Position (meters) + double _positionX = 0.0; double _positionY = 0.0; double _positionZ = 0.0; DateTime? _lastIntegrationTime; - // Orientation tracking via gyroscope integration + // Orientation (radians) double _orientationX = 0.0; // Roll (rotation around X-axis, in radians) double _orientationY = 0.0; // Pitch (rotation around Y-axis, in radians) double _orientationZ = 0.0; // Yaw (rotation around Z-axis, in radians) DateTime? _lastGyroIntegrationTime; - // Reference positions (set when "Set Reference" is pressed) - Pose? _referenceArmPose; // Arm position when reference was set + // Arm position in the world when we set the reference. set once when you press "set reference" and stays constant. + Pose? _referenceArmPose; bool _isReferenceSet = false; - - // Current arm position (for display) + // the latest postion of the arm, updates everytime the arm moves. used to display the arm's position. Pose? _currentArmPose; _initAccelerometer() { @@ -100,47 +103,10 @@ class _ImuWidgetState extends State { ); } - /// Update orientation by integrating gyroscope angular velocity - void _updateOrientationFromGyroscope(GyroscopeEvent event) { - final now = DateTime.now(); - - // Initialize integration time on first run - if (_lastGyroIntegrationTime == null) { - _lastGyroIntegrationTime = now; - return; - } - - // Calculate time delta for integration (in seconds) - final dt = now.difference(_lastGyroIntegrationTime!).inMilliseconds / 1000.0; - _lastGyroIntegrationTime = now; - - // Skip if dt is too large (indicates pause or first run) - if (dt > 0.5) { - return; - } - - // Don't update orientation if reference point hasn't been set - if (!_isReferenceSet) { - return; - } - - // Integrate angular velocity to get orientation change - // Gyroscope values are in radians/second - // event.x = rotation rate around X-axis (roll) - // event.y = rotation rate around Y-axis (pitch) - // event.z = rotation rate around Z-axis (yaw) - _orientationX += event.x * dt; - _orientationY += event.y * dt; - _orientationZ += event.z * dt; - - // Optional: Apply small decay to prevent drift - _orientationX *= 0.999; - _orientationY *= 0.999; - _orientationZ *= 0.999; - } - - /// Move the arm based on IMU accelerometer data by integrating acceleration to position + /// Move the arm based on IMU accelerometer data using acceleration to get position + /// we do this by taking integral of acceleration to get velocity, and then integral of velocity to get position. Future _moveArmFromImu(UserAccelerometerEvent event) async { + // event is the accelerometer data from the phone ^^ final now = DateTime.now(); // Initialize integration time on first run @@ -149,36 +115,42 @@ class _ImuWidgetState extends State { return; } - // Calculate time delta for integration (in seconds) + // Calculate time delta between now and last integration time (in seconds) + // why? b/c we need to know how much time has passed, aka how long have we been accelerating for? + // tldr: to get velocity, we need acceleration * time. final dt = now.difference(_lastIntegrationTime!).inMilliseconds / 1000.0; _lastIntegrationTime = now; - // Skip if dt is too large (indicates pause or first run) + // Skip if dt is too large, meaning the phone has been stationary for too long in between movements. if (dt > 0.5) { + // this number might be too small return; } // Apply dead zone to acceleration + // peoples hands are shaky, phone sensors are jittery, this helps filter out the noise. final accelX = event.x.abs() > _deadZone ? event.x : 0.0; final accelY = event.y.abs() > _deadZone ? event.y : 0.0; final accelZ = event.z.abs() > _deadZone ? event.z : 0.0; - // STEP 1: Integrate acceleration to get velocity (v = v0 + a*dt) + // STEP 1: Calculate velocity. velocity is the integral of acceleration wrt time. (v = v0 + a*dt) _velocityX += accelX * dt; _velocityY += accelY * dt; _velocityZ += accelZ * dt; // Apply decay to prevent drift when stationary + // we are multiply velocity by 0.95 so that we reduce it by 5%. _velocityX *= _velocityDecay; _velocityY *= _velocityDecay; _velocityZ *= _velocityDecay; - // STEP 2: Integrate velocity to get position (p = p0 + v*dt) + // STEP 2: Caluclate position. position is the integral of velocity wrt time. (p = p0 + v*dt) _positionX += _velocityX * dt; _positionY += _velocityY * dt; _positionZ += _velocityZ * dt; - // Rate limiting: don't send arm commands too frequently + // Rate limiting, don't send arm commands too frequently. + // this prevents us from sending too many commands to the arm too quickly. if (_lastArmCommandTime != null) { final timeSinceLastCommand = now.difference(_lastArmCommandTime!); if (timeSinceLastCommand < _armCommandInterval) { @@ -196,19 +168,25 @@ class _ImuWidgetState extends State { return; } + // ready to move the arm! + // mark that we are about to send a command. + // save the current time (for rate limiting next time) + // set the "busy" flag to true _lastArmCommandTime = now; _isMovingArm = true; try { // 3. Calculate new target position based on reference + phone displacement // Reference arm position + (phone displacement * scale factor) - // Phone displacement is in meters, arm position is in mm, so scale appropriately + // remeber: referenceArmPose is the arm's position in the real world when we set the reference. + // positionScale = 50mm/meter, so if we move the phone 1 meter, the arm will move 50mm. final newX = _referenceArmPose!.x + (_positionX * _positionScale); final newY = _referenceArmPose!.y + (_positionY * _positionScale); final newZ = _referenceArmPose!.z + (_positionZ * _positionScale); - // 4. Create new pose with orientation from gyroscope - // Combine reference orientation with the accumulated orientation changes + // 4. Create new pose with position and orientation + // positions are what we calculated right above + // orientations are calculated from the reference plus the orientation changes from the gyroscope. final newPose = Pose( x: newX, y: newY, @@ -236,14 +214,57 @@ class _ImuWidgetState extends State { } } - /// Set reference point: links current phone position to current arm position + /// Update orientation by integrating gyroscope angular velocity + void _updateOrientationFromGyroscope(GyroscopeEvent event) { + final now = DateTime.now(); + + // Initialize integration time on first run + if (_lastGyroIntegrationTime == null) { + _lastGyroIntegrationTime = now; + return; + } + + // Calculate time delta between now and last integration time (in seconds) so we know how long we've been rotating for. + // tldr: to get orientation, we need angular velocity * time. + final dt = now.difference(_lastGyroIntegrationTime!).inMilliseconds / 1000.0; + _lastGyroIntegrationTime = now; + + // Skip if dt is too large, meaning the phone has been stationary for too long in between movements. + if (dt > 0.5) { + return; + } + + // Don't update orientation if reference point hasn't been set + if (!_isReferenceSet) { + return; + } + + // Calucluate orientation change: integrate angular velocity over time to get orientation (angle). + // Gyroscope values are in radians/second + // event.x = rotation rate around X-axis (roll) + // event.y = rotation rate around Y-axis (pitch) + // event.z = rotation rate around Z-axis (yaw) + _orientationX += event.x * dt; + _orientationY += event.y * dt; + _orientationZ += event.z * dt; + + // Apply small decay to prevent drift + // the 0.999 is aritrary for now + _orientationX *= 0.999; + _orientationY *= 0.999; + _orientationZ *= 0.999; + } + + /// Set reference point + /// Gets the arm's current position in the real world and stores it as the reference. + /// Clears position and orientation tracking so the phone starts at (0,0,0) relative to this reference. Future _setReference() async { try { // Get the current arm position final currentArmPose = await widget.arm.endPosition(); setState(() { - // Zero out the phone position tracking (this is the new starting point) + // Zero out the phone position tracking _positionX = 0.0; _positionY = 0.0; _positionZ = 0.0; From d9573525fbcdd45ae19dc4317aa53a9bac3b8220 Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Wed, 5 Nov 2025 16:08:56 -0500 Subject: [PATCH 06/27] decently working state at 408 on wednesday --- .../viam_example_app/ios/Runner/Info.plist | 2 + .../lib/resources/arm_widgets/imu_widget.dart | 258 ++++++++++++++---- 2 files changed, 200 insertions(+), 60 deletions(-) diff --git a/example/viam_example_app/ios/Runner/Info.plist b/example/viam_example_app/ios/Runner/Info.plist index 1ad391ce58f..7c7c78ebdbb 100644 --- a/example/viam_example_app/ios/Runner/Info.plist +++ b/example/viam_example_app/ios/Runner/Info.plist @@ -8,6 +8,8 @@ _rpc._tcp + NSMotionUsageDescription + This app requires access to the barometer to provide altitude information. CADisableMinimumFrameDurationOnPhone CFBundleDevelopmentRegion diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index c4246e5af0f..73575237f7f 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -1,8 +1,11 @@ import 'dart:async'; +import 'dart:math' as math; import 'package:flutter/material.dart'; import 'package:sensors_plus/sensors_plus.dart'; +import 'package:vector_math/vector_math.dart' as vector_math; import 'package:viam_example_app/resources/arm_screen.dart'; +import 'package:viam_sdk/protos/app/robot.dart'; import 'package:viam_sdk/viam_sdk.dart'; class ImuWidget extends StatefulWidget { @@ -29,13 +32,13 @@ class _ImuWidgetState extends State { // Arm movement control variables DateTime? _lastArmCommandTime; - static const Duration _armCommandInterval = Duration(milliseconds: 100); // Rate limit: 10 commands/sec - - // Scale factor: 50mm/meter = 50mm of arm movement per meter of phone movement - static const double _positionScale = 50.0; + static const Duration _armCommandInterval = Duration(milliseconds: 200); // Rate limit: 10 commands/sec - static const double _velocityDecay = 0.95; // Decay factor to prevent drift - static const double _deadZone = 0.5; // Ignore small accelerations + // Scale factor: 300mm/meter = 300mm of arm movement per meter of phone movement + static const double _positionScale = 300.0; + + static const double _velocityDecay = 0.90; // Decay factor to prevent drift + static const double _deadZone = 0.3; // Ignore small accelerations bool _isMovingArm = false; String? _lastError; @@ -103,6 +106,47 @@ class _ImuWidgetState extends State { ); } + /// Update orientation by integrating gyroscope angular velocity + void _updateOrientationFromGyroscope(GyroscopeEvent event) { + final now = DateTime.now(); + + // Initialize integration time on first run + if (_lastGyroIntegrationTime == null) { + _lastGyroIntegrationTime = now; + return; + } + + // Calculate time delta between now and last integration time (in seconds) so we know how long we've been rotating for. + // tldr: to get orientation, we need angular velocity * time. + final dt = now.difference(_lastGyroIntegrationTime!).inMilliseconds / 1000.0; + _lastGyroIntegrationTime = now; + + // Skip if dt is too large, meaning the phone has been stationary for too long in between movements. + if (dt > 0.5) { + return; + } + + // Don't update orientation if reference point hasn't been set + if (!_isReferenceSet) { + return; + } + + // Calucluate orientation change: integrate angular velocity over time to get orientation (angle). + // Gyroscope values are in radians/second + // event.x = rotation rate around X-axis (roll) + // event.y = rotation rate around Y-axis (pitch) + // event.z = rotation rate around Z-axis (yaw) + _orientationX += event.x * dt; + _orientationY += event.y * dt; + _orientationZ += event.z * dt; + + // Apply small decay to prevent drift + // the 0.999 is aritrary for now + _orientationX *= 0.999; + _orientationY *= 0.999; + _orientationZ *= 0.999; + } + /// Move the arm based on IMU accelerometer data using acceleration to get position /// we do this by taking integral of acceleration to get velocity, and then integral of velocity to get position. Future _moveArmFromImu(UserAccelerometerEvent event) async { @@ -122,16 +166,23 @@ class _ImuWidgetState extends State { _lastIntegrationTime = now; // Skip if dt is too large, meaning the phone has been stationary for too long in between movements. - if (dt > 0.5) { - // this number might be too small - return; - } + // if (dt > 0.5) { + // // this number might be too small + // return; + // } // Apply dead zone to acceleration // peoples hands are shaky, phone sensors are jittery, this helps filter out the noise. final accelX = event.x.abs() > _deadZone ? event.x : 0.0; final accelY = event.y.abs() > _deadZone ? event.y : 0.0; final accelZ = event.z.abs() > _deadZone ? event.z : 0.0; + // print('eventz: ${event.z}'); + if (accelX == 0.0 && accelY == 0.0 && accelZ == 0.0) { + _velocityX = 0.0; + _velocityY = 0.0; + _velocityZ = 0.0; + // return; + } // STEP 1: Calculate velocity. velocity is the integral of acceleration wrt time. (v = v0 + a*dt) _velocityX += accelX * dt; @@ -146,8 +197,11 @@ class _ImuWidgetState extends State { // STEP 2: Caluclate position. position is the integral of velocity wrt time. (p = p0 + v*dt) _positionX += _velocityX * dt; + // print('positionX: $_positionX'); _positionY += _velocityY * dt; + // print('positionY: $_positionY'); _positionZ += _velocityZ * dt; + print('positionZ: $_positionZ'); // Rate limiting, don't send arm commands too frequently. // this prevents us from sending too many commands to the arm too quickly. @@ -163,7 +217,7 @@ class _ImuWidgetState extends State { return; } - // Don't move arm if reference point hasn't been set + // // Don't move arm if reference point hasn't been set if (!_isReferenceSet || _referenceArmPose == null) { return; } @@ -180,30 +234,54 @@ class _ImuWidgetState extends State { // Reference arm position + (phone displacement * scale factor) // remeber: referenceArmPose is the arm's position in the real world when we set the reference. // positionScale = 50mm/meter, so if we move the phone 1 meter, the arm will move 50mm. - final newX = _referenceArmPose!.x + (_positionX * _positionScale); - final newY = _referenceArmPose!.y + (_positionY * _positionScale); - final newZ = _referenceArmPose!.z + (_positionZ * _positionScale); + final newX = _referenceArmPose!.x + (_positionY * _positionScale); // flipped x and y + final newY = (_referenceArmPose!.y + (_positionX * _positionScale)) * -1; // flipped y + final newZ = (_referenceArmPose!.z + (_positionZ * _positionScale)); // flipped z + + // TODO: convert euler angles to orientation vector using spatial math package + + // final quaternion = vector_math.Quaternion.euler(_orientationZ, _orientationY, _orientationX); // Yaw, Pitch, Roll + // final quaternion = math.Quaternion(_orientationZ, _orientationY, _orientationX, 0.0); + final quaternion = vector_math.Quaternion.identity(); + quaternion.setEuler(_orientationZ, _orientationY, _orientationX); // Yaw, Pitch, Roll + final orientationVector = quatToOV(quaternion); + final newOrientationX = _referenceArmPose!.oX + orientationVector.x; + final newOrientationY = _referenceArmPose!.oY + orientationVector.y; + final newOrientationZ = _referenceArmPose!.oZ + orientationVector.z; // 4. Create new pose with position and orientation // positions are what we calculated right above // orientations are calculated from the reference plus the orientation changes from the gyroscope. final newPose = Pose( - x: newX, + x: newX, // flipped x and y y: newY, z: newZ, theta: _referenceArmPose!.theta, // Keep theta from reference - oX: _referenceArmPose!.oX + _orientationX, - oY: _referenceArmPose!.oY + _orientationY, - oZ: _referenceArmPose!.oZ + _orientationZ, + // oX: _referenceArmPose!.oX + _orientationX, // these are probs wrong bc we are adding orientation vector values + ueler values + // oY: _referenceArmPose!.oY + _orientationY, + // oZ: _referenceArmPose!.oZ + _orientationZ, + oX: newOrientationX, + oY: newOrientationY, + oZ: newOrientationZ, ); + if (newPose.x == _currentArmPose!.x && + newPose.y == _currentArmPose!.y && + newPose.z == _currentArmPose!.z && + newPose.oX == _currentArmPose!.oX && + newPose.oY == _currentArmPose!.oY && + newPose.oZ == _currentArmPose!.oZ) { + return; + } + // 5. Move the arm to the new position + // if (!await widget.arm.isMoving()) { await widget.arm.moveToPosition(newPose); - setState(() { _lastError = null; _currentArmPose = newPose; // Store for display }); + // } } catch (e) { // Handle errors gracefully setState(() { @@ -214,47 +292,6 @@ class _ImuWidgetState extends State { } } - /// Update orientation by integrating gyroscope angular velocity - void _updateOrientationFromGyroscope(GyroscopeEvent event) { - final now = DateTime.now(); - - // Initialize integration time on first run - if (_lastGyroIntegrationTime == null) { - _lastGyroIntegrationTime = now; - return; - } - - // Calculate time delta between now and last integration time (in seconds) so we know how long we've been rotating for. - // tldr: to get orientation, we need angular velocity * time. - final dt = now.difference(_lastGyroIntegrationTime!).inMilliseconds / 1000.0; - _lastGyroIntegrationTime = now; - - // Skip if dt is too large, meaning the phone has been stationary for too long in between movements. - if (dt > 0.5) { - return; - } - - // Don't update orientation if reference point hasn't been set - if (!_isReferenceSet) { - return; - } - - // Calucluate orientation change: integrate angular velocity over time to get orientation (angle). - // Gyroscope values are in radians/second - // event.x = rotation rate around X-axis (roll) - // event.y = rotation rate around Y-axis (pitch) - // event.z = rotation rate around Z-axis (yaw) - _orientationX += event.x * dt; - _orientationY += event.y * dt; - _orientationZ += event.z * dt; - - // Apply small decay to prevent drift - // the 0.999 is aritrary for now - _orientationX *= 0.999; - _orientationY *= 0.999; - _orientationZ *= 0.999; - } - /// Set reference point /// Gets the arm's current position in the real world and stores it as the reference. /// Clears position and orientation tracking so the phone starts at (0,0,0) relative to this reference. @@ -351,4 +388,105 @@ class _ImuWidgetState extends State { subscription.cancel(); } } + + /// Converts a unit quaternion (q) to an OrientationVector. + /// + /// q: The input rotation quaternion. (Dart: (x, y, z, w) = (Imag, Jmag, Kmag, Real)) + /// Converted from go code to flutter using gemini + Orientation_OrientationVectorRadians quatToOV(vector_math.Quaternion q) { + double orientationVectorPoleRadius = 0.0001; + double defaultAngleEpsilon = 1e-4; + // Define initial axes as pure quaternions (Real/W=0) + // xAxis: (0, -1, 0, 0) -> x=-1, y=0, z=0, w=0 + final vector_math.Quaternion xAxis = vector_math.Quaternion(0.0, -1.0, 0.0, 0.0); + // zAxis: (0, 0, 0, 1) -> x=0, y=0, z=1, w=0 + final vector_math.Quaternion zAxis = vector_math.Quaternion(0.0, 0.0, 0.0, 1.0); + + final ov = Orientation_OrientationVectorRadians(); + + // 1. Get the transform of our +X and +Z points (Quaternion rotation formula: q * v * q_conj) + final vector_math.Quaternion newX = q * xAxis * q.conjugated(); + final vector_math.Quaternion newZ = q * zAxis * q.conjugated(); + + // Set the direction vector (OX, OY, OZ) from the rotated Z-axis (Imag, Jmag, Kmag components) + ov.x = newZ.x; + ov.y = newZ.y; + ov.z = newZ.z; + + // 2. Calculate the roll angle (Theta) + + // Check if we are near the poles (i.e., newZ.z/Kmag is close to 1 or -1) + if (1 - (ov.z.abs()) > orientationVectorPoleRadius) { + // --- General Case: Not Near the Pole --- + + // Vector3 versions of the rotated axes + final vector_math.Vector3 v1 = vector_math.Vector3(newZ.x, newZ.y, newZ.z); // Local Z + final vector_math.Vector3 v2 = vector_math.Vector3(newX.x, newX.y, newX.z); // Local X + final vector_math.Vector3 globalZ = vector_math.Vector3(0.0, 0.0, 1.0); // Global Z + + // Normal to the local-x, local-z plane + final vector_math.Vector3 norm1 = v1.cross(v2); + + // Normal to the global-z, local-z plane + final vector_math.Vector3 norm2 = v1.cross(globalZ); + + // Find the angle (theta) between the two planes (using the angle between their normals) + final double denominator = norm1.length * norm2.length; + final double cosTheta = denominator != 0.0 ? norm1.dot(norm2) / denominator : 1.0; // Avoid division by zero, default to 1 (0 angle) + + // Clamp for float error + double clampedCosTheta = cosTheta.clamp(-1.0, 1.0); + + final double theta = math.acos(clampedCosTheta); + + if (theta.abs() > orientationVectorPoleRadius) { + // Determine directionality of the angle (sign of theta) + + // Axis is the new Z-axis (ov.OX, ov.OY, ov.OZ) + final vector_math.Vector3 axis = vector_math.Vector3(ov.x, ov.y, ov.z); + // Create a rotation quaternion for rotation by -theta around the new Z-axis + final vector_math.Quaternion q2 = vector_math.Quaternion.axisAngle(axis, -theta); + + // Apply q2 rotation to the original Z-axis (0, 0, 0, 1) + final vector_math.Quaternion testZQuat = q2 * zAxis * q2.conjugated(); + final vector_math.Vector3 testZVector = vector_math.Vector3(testZQuat.x, testZQuat.y, testZQuat.z); + + // Find the normal of the plane defined by v1 (local Z) and testZ + final vector_math.Vector3 norm3 = v1.cross(testZVector); + + final double norm1Len = norm1.length; + final double norm3Len = norm3.length; + + final double cosTest = (norm1Len * norm3Len) != 0.0 ? norm1.dot(norm3) / (norm1Len * norm3Len) : 1.0; + + // Check if norm1 and norm3 are coplanar (angle close to 0) + if (1.0 - cosTest.abs() < defaultAngleEpsilon * defaultAngleEpsilon) { + ov.theta = -theta; + } else { + ov.theta = theta; + } + } else { + ov.theta = 0.0; + } + } else { + // --- Special Case: Near the Pole (Z-axis is up or down) --- + + // Use Atan2 on the rotated X-axis components (Jmag and Imag, or y and x in Dart) + // -math.Atan2(newX.Jmag, -newX.Imag) -> Dart: -math.atan2(newX.y, -newX.x) + ov.theta = -math.atan2(newX.y, -newX.x); + + if (newZ.z < 0) { + // If pointing along the negative Z-axis (ov.OZ < 0) + // -math.Atan2(newX.Jmag, newX.Imag) -> Dart: -math.atan2(newX.y, newX.x) + ov.theta = -math.atan2(newX.y, newX.x); + } + } + + // Handle IEEE -0.0 for consistency + if (ov.theta == -0.0) { + ov.theta = 0.0; + } + + return ov; + } } From 6d38649090f6700d9095e81b5c25d1599eccb776 Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Wed, 5 Nov 2025 17:04:53 -0500 Subject: [PATCH 07/27] queue --- .../lib/resources/arm_widgets/imu_widget.dart | 63 ++++++++++++++++--- 1 file changed, 54 insertions(+), 9 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index 73575237f7f..c829ea94240 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -35,13 +35,18 @@ class _ImuWidgetState extends State { static const Duration _armCommandInterval = Duration(milliseconds: 200); // Rate limit: 10 commands/sec // Scale factor: 300mm/meter = 300mm of arm movement per meter of phone movement - static const double _positionScale = 300.0; + static const double _positionScale = 1000.0; static const double _velocityDecay = 0.90; // Decay factor to prevent drift static const double _deadZone = 0.3; // Ignore small accelerations bool _isMovingArm = false; String? _lastError; + // Pose queue for batching arm movements + final List _poseQueue = []; + int _poseCounter = 0; + bool _isProcessingQueue = false; + // Velocity (meters per second) double _velocityX = 0.0; double _velocityY = 0.0; @@ -274,14 +279,14 @@ class _ImuWidgetState extends State { return; } - // 5. Move the arm to the new position - // if (!await widget.arm.isMoving()) { - await widget.arm.moveToPosition(newPose); - setState(() { - _lastError = null; - _currentArmPose = newPose; // Store for display - }); - // } + // Add pose to queue + _poseQueue.add(newPose); + _poseCounter++; + + // Start processing queue if not already processing + if (!_isProcessingQueue) { + _processQueueSequentially(); + } } catch (e) { // Handle errors gracefully setState(() { @@ -292,6 +297,40 @@ class _ImuWidgetState extends State { } } + /// Process pose queue sequentially, executing every 5th pose + Future _processQueueSequentially() async { + _isProcessingQueue = true; + + while (_poseQueue.isNotEmpty) { + // Only execute every 5th pose + if (_poseCounter % 5 == 0) { + // Get the latest pose from the queue (skip intermediate ones) + final poseToExecute = _poseQueue.last; + _poseQueue.clear(); // Clear all accumulated poses + + try { + await widget.arm.moveToPosition(poseToExecute); + setState(() { + _lastError = null; + _currentArmPose = poseToExecute; // Store for display + }); + } catch (e) { + setState(() { + _lastError = e.toString(); + }); + } + } else { + // Skip this batch, just clear the queue + _poseQueue.clear(); + } + + // Small delay to allow new poses to accumulate + await Future.delayed(const Duration(milliseconds: 50)); + } + + _isProcessingQueue = false; + } + /// Set reference point /// Gets the arm's current position in the real world and stores it as the reference. /// Clears position and orientation tracking so the phone starts at (0,0,0) relative to this reference. @@ -316,6 +355,10 @@ class _ImuWidgetState extends State { _orientationZ = 0.0; _lastGyroIntegrationTime = null; + // Clear pose queue and reset counter + _poseQueue.clear(); + _poseCounter = 0; + // Store the current arm position as the reference _referenceArmPose = currentArmPose; _currentArmPose = currentArmPose; // Also store for display @@ -387,6 +430,8 @@ class _ImuWidgetState extends State { for (final subscription in _streamSubscriptions) { subscription.cancel(); } + // Clear pose queue on dispose + _poseQueue.clear(); } /// Converts a unit quaternion (q) to an OrientationVector. From 577f91897c8f3788a5f20f64a0295df0ebc2dd1e Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Wed, 5 Nov 2025 17:07:19 -0500 Subject: [PATCH 08/27] clean up pr --- .../lib/resources/arm_screen.dart | 9 - .../arm_widgets/angular_arrows_widget.dart | 380 ------------------ .../arm_widgets/joint_positions_widget.dart | 218 ---------- .../arm_widgets/linear_arrows_widget.dart | 242 ----------- .../arm_widgets/orienation_widget.dart | 266 ------------ .../arm_widgets/position_widget.dart | 240 ----------- example/viam_example_app/pubspec.yaml | 1 + 7 files changed, 1 insertion(+), 1355 deletions(-) delete mode 100644 example/viam_example_app/lib/resources/arm_widgets/angular_arrows_widget.dart delete mode 100644 example/viam_example_app/lib/resources/arm_widgets/joint_positions_widget.dart delete mode 100644 example/viam_example_app/lib/resources/arm_widgets/linear_arrows_widget.dart delete mode 100644 example/viam_example_app/lib/resources/arm_widgets/orienation_widget.dart delete mode 100644 example/viam_example_app/lib/resources/arm_widgets/position_widget.dart diff --git a/example/viam_example_app/lib/resources/arm_screen.dart b/example/viam_example_app/lib/resources/arm_screen.dart index 2b16784e218..2ef364a4454 100644 --- a/example/viam_example_app/lib/resources/arm_screen.dart +++ b/example/viam_example_app/lib/resources/arm_screen.dart @@ -1,8 +1,5 @@ import 'package:flutter/material.dart'; import 'package:viam_example_app/resources/arm_widgets/imu_widget.dart'; -import 'package:viam_example_app/resources/arm_widgets/joint_positions_widget.dart'; -import 'package:viam_example_app/resources/arm_widgets/orienation_widget.dart'; -import 'package:viam_example_app/resources/arm_widgets/position_widget.dart'; import 'package:viam_sdk/viam_sdk.dart'; /// A widget to control an [Arm]. @@ -19,12 +16,6 @@ class ViamArmWidgetNew extends StatelessWidget { Widget build(BuildContext context) { return Column( children: [ - JointPositionsWidget(arm: arm), - // LinearArrowsWidget(), - PositionWidget(arm: arm), - // AngularArrowsWidget(), - // JointPositionsWidget(arm: arm), - OrientationWidget(arm: arm), ImuWidget(arm: arm, updateNotifier: ArmNotifier()), ], ); diff --git a/example/viam_example_app/lib/resources/arm_widgets/angular_arrows_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/angular_arrows_widget.dart deleted file mode 100644 index 9015381db1e..00000000000 --- a/example/viam_example_app/lib/resources/arm_widgets/angular_arrows_widget.dart +++ /dev/null @@ -1,380 +0,0 @@ -import 'package:flutter/material.dart'; - -final size = 300.0; - -class AngularArrowsWidget extends StatelessWidget { - const AngularArrowsWidget({super.key}); - - @override - Widget build(BuildContext context) { - return Column( - children: [ - Divider(), - Text( - 'End-effector Orientation', - style: TextStyle( - fontWeight: FontWeight.bold, - ), - ), - Divider(), - SizedBox(height: 40), - Stack( - children: [ - _CurvedArrowPad( - // TODO: add functions for arrow functionality - onUp: () {}, - onDown: () {}, - onLeft: () {}, - onRight: () {}, - ), - _BuildCornerButton( - direction: ArcDirection.left, - label: 'RZ+', - onPressed: () {}, - ), - _BuildCornerButton( - direction: ArcDirection.right, - label: 'RZ-', - onPressed: () {}, - ), - ], - ), - ], - ); - } -} - -enum ArrowDirection { up, down, left, right } - -enum ArcDirection { left, right } - -class _Corners { - final Offset topLeft; - final Offset topRight; - final Offset bottomRight; - final Offset bottomLeft; - - _Corners({ - required this.topLeft, - required this.topRight, - required this.bottomRight, - required this.bottomLeft, - }); -} - -class _CurvedArrowPad extends StatelessWidget { - final VoidCallback? onUp; - final VoidCallback? onDown; - final VoidCallback? onLeft; - final VoidCallback? onRight; - - const _CurvedArrowPad({ - this.onUp, - this.onDown, - this.onLeft, - this.onRight, - }); - - @override - Widget build(BuildContext context) { - return Center( - child: SizedBox( - height: size, - width: size, - child: Stack( - children: [ - _BuildArrowButton(alignment: Alignment.topCenter, direction: ArrowDirection.up, onPressed: onUp, label: 'RX-'), - _BuildArrowButton(alignment: Alignment.bottomCenter, direction: ArrowDirection.down, onPressed: onDown, label: 'RX+'), - _BuildArrowButton(alignment: Alignment.centerLeft, direction: ArrowDirection.left, onPressed: onLeft, label: 'RY-'), - _BuildArrowButton(alignment: Alignment.centerRight, direction: ArrowDirection.right, onPressed: onRight, label: 'RY+'), - ], - ), - ), - ); - } -} - -class _BuildArrowButton extends StatelessWidget { - final Alignment alignment; - final ArrowDirection direction; - final String label; - final VoidCallback? onPressed; - - const _BuildArrowButton({ - required this.alignment, - required this.direction, - required this.onPressed, - required this.label, - }); - - @override - Widget build(BuildContext context) { - return Align( - alignment: alignment, - child: SizedBox( - width: size / 2.5, - height: size / 2.5, - child: IconButton( - icon: Stack( - alignment: Alignment.center, - children: [ - CustomPaint( - painter: _AngularArrowPainter(direction: direction, color: Colors.black), - child: const SizedBox.expand(), - ), - Text( - label, - style: const TextStyle( - color: Colors.white, - fontWeight: FontWeight.bold, - fontSize: 16, - ), - ), - ], - ), - onPressed: onPressed, - ), - ), - ); - } -} - -class _AngularArrowPainter extends CustomPainter { - final ArrowDirection direction; - final Color color; - - _AngularArrowPainter({required this.direction, required this.color}); - - @override - void paint(Canvas canvas, Size size) { - final paint = Paint() - ..color = color - ..style = PaintingStyle.fill; - - final path = Path(); - final w = size.width; - final h = size.height; - - final double offset = 0.2; - final double offsetInverse = 0.8; - - final double length = 0.3; - final double curveFactor = 0.1; - - final double curvePos = offset + curveFactor; - final double pointPos = offset + length; - - final double tipCurvePosInverse = offsetInverse - curveFactor; - final double headPosInverse = offsetInverse - length; - - switch (direction) { - case ArrowDirection.up: - path.moveTo(w / 2, h * offset); - path.quadraticBezierTo(w * (1 - curveFactor), h * curvePos, w, h * pointPos); - - path.lineTo(w * 0.8, h * pointPos); - path.lineTo(w * 0.8, h); - path.lineTo(w * 0.2, h); - path.lineTo(w * 0.2, h * pointPos); - path.lineTo(0, h * pointPos); - - path.quadraticBezierTo(w * curveFactor, h * curvePos, w / 2, h * offset); - break; - case ArrowDirection.down: - path.moveTo(w / 2, h * offsetInverse); - - path.quadraticBezierTo(w * curveFactor, h * tipCurvePosInverse, 0, h * headPosInverse); - - path.lineTo(w * 0.2, h * headPosInverse); - path.lineTo(w * 0.2, 0); - path.lineTo(w * 0.8, 0); - path.lineTo(w * 0.8, h * headPosInverse); - path.lineTo(w, h * headPosInverse); - - path.quadraticBezierTo(w * (1 - curveFactor), h * tipCurvePosInverse, w / 2, h * offsetInverse); - break; - case ArrowDirection.left: - path.moveTo(w * offset, h / 2); - - path.quadraticBezierTo(w * curvePos, h * curveFactor, w * pointPos, 0); - - path.lineTo(w * pointPos, h * 0.2); - path.lineTo(w, h * 0.2); - path.lineTo(w, h * 0.8); - path.lineTo(w * pointPos, h * 0.8); - path.lineTo(w * pointPos, h); - - path.quadraticBezierTo(w * curvePos, h * (1 - curveFactor), w * offset, h / 2); - break; - case ArrowDirection.right: - path.moveTo(w * offsetInverse, h / 2); - - path.quadraticBezierTo(w * tipCurvePosInverse, h * (1 - curveFactor), w * headPosInverse, h); - path.lineTo(w * headPosInverse, h * 0.8); - path.lineTo(0, h * 0.8); - path.lineTo(0, h * 0.2); - path.lineTo(w * headPosInverse, h * 0.2); - path.lineTo(w * headPosInverse, 0); - - path.quadraticBezierTo(w * tipCurvePosInverse, h * curveFactor, w * offsetInverse, h / 2); - break; - } - path.close(); - canvas.drawPath(path, paint); - } - - @override - bool shouldRepaint(covariant _AngularArrowPainter oldDelegate) => false; -} - -class _BuildCornerButton extends StatelessWidget { - final ArcDirection direction; - final String label; - final VoidCallback onPressed; - - const _BuildCornerButton({ - required this.direction, - required this.label, - required this.onPressed, - }); - - @override - Widget build(BuildContext context) { - return Align( - alignment: Alignment.center, - child: Padding( - padding: const EdgeInsets.all(24.0), - child: SizedBox( - width: 100, - height: 100, - child: Stack( - children: [ - CustomPaint( - painter: _ArcArrowPainter(sign: direction == ArcDirection.left ? -1 : 1, label: label), - child: const SizedBox.expand(), - ), - ], - ), - ), - ), - ); - } -} - -(Offset, Offset) _getControlPoints(_Corners corners, double sign) { - const double arcHeight = 45.0; - - final topChordLength = (corners.topRight - corners.topLeft).distance; - final bottomChordLength = (corners.bottomRight - corners.bottomLeft).distance; - final bottomArcHeight = arcHeight * (bottomChordLength / topChordLength); - - final topMidpoint = Offset( - (corners.topLeft.dx + corners.topRight.dx) / 2, - (corners.topLeft.dy + corners.topRight.dy) / 2, - ); - final topPerpendicular = Offset(corners.topRight.dy - corners.topLeft.dy, -(corners.topRight.dx - corners.topLeft.dx)) * sign; - final topNormal = _normalize(topPerpendicular); - final topControlPoint = topMidpoint + topNormal * arcHeight; - - final bottomMidpoint = Offset( - (corners.bottomRight.dx + corners.bottomLeft.dx) / 2, - (corners.bottomRight.dy + corners.bottomLeft.dy) / 2, - ); - final bottomPerpendicular = - Offset(-(corners.bottomLeft.dy - corners.bottomRight.dy), corners.bottomLeft.dx - corners.bottomRight.dx) * sign; - final bottomNormal = _normalize(bottomPerpendicular); - final bottomControlPoint = bottomMidpoint + bottomNormal * bottomArcHeight; - - return (topControlPoint, bottomControlPoint); -} - -(Offset, Offset, Offset) _getArrowPoints(_Corners corners, double sign) { - const double arrowheadLength = 40.0; - const double shoulderWidth = 40.0; - - final endMidpoint = Offset((corners.topRight.dx + corners.bottomRight.dx) / 2, (corners.topRight.dy + corners.bottomRight.dy) / 2); - final endVector = corners.bottomRight - corners.topRight; - final endDirection = _normalize(endVector); - - final outwardVector = Offset(endVector.dy, -endVector.dx) * sign; - final outwardNormal = _normalize(outwardVector); - - final arrowPoint = endMidpoint + outwardNormal * arrowheadLength; - final shoulderTop = endMidpoint - endDirection * shoulderWidth; - final shoulderBottom = endMidpoint + endDirection * shoulderWidth; - - return (arrowPoint, shoulderTop, shoulderBottom); -} - -Offset _getTextPosition(Offset shoulderTop, Offset shoulderBottom, double width, double height) { - final midpoint = Offset((shoulderTop.dx + shoulderBottom.dx) / 2, (shoulderTop.dy + shoulderBottom.dy) / 2); - return Offset(midpoint.dx - width / 2, midpoint.dy - height / 2); -} - -Offset _normalize(Offset o) { - final d = o.distance; - if (d == 0) return Offset.zero; - return Offset(o.dx / d, o.dy / d); -} - -class _ArcArrowPainter extends CustomPainter { - final double sign; - final String label; - _ArcArrowPainter({required this.sign, required this.label}); - - @override - void paint(Canvas canvas, Size size) { - canvas.translate(size.width / 2, 0); - - final fillPaint = Paint() - ..color = Colors.black - ..style = PaintingStyle.fill; - - // Corners of the arc rectangle - final corners = _Corners( - topLeft: Offset(sign * 5, -50), - topRight: Offset(sign * 165, 30), - bottomRight: Offset(sign * 133, 54), - bottomLeft: Offset(sign * 5, -10), - ); - - final textPainter = TextPainter( - text: TextSpan( - text: label, - style: TextStyle(color: Colors.white, fontWeight: FontWeight.bold, fontSize: 16), - ), - textAlign: TextAlign.center, - textDirection: TextDirection.ltr, - )..layout(); - - Offset topControlPoint; - Offset bottomControlPoint; - Offset arrowPoint; - Offset shoulderTop; - Offset shoulderBottom; - Offset textPosition; - - (topControlPoint, bottomControlPoint) = _getControlPoints(corners, sign); - (arrowPoint, shoulderTop, shoulderBottom) = _getArrowPoints(corners, sign); - textPosition = _getTextPosition(shoulderTop, shoulderBottom, textPainter.width, textPainter.height); - - // Draw arrows - final path = Path() - ..moveTo(corners.topLeft.dx, corners.topLeft.dy) - ..quadraticBezierTo(topControlPoint.dx, topControlPoint.dy, corners.topRight.dx, corners.topRight.dy) - ..lineTo(shoulderTop.dx, shoulderTop.dy) - ..lineTo(arrowPoint.dx, arrowPoint.dy) - ..lineTo(shoulderBottom.dx, shoulderBottom.dy) - ..lineTo(corners.bottomRight.dx, corners.bottomRight.dy) - ..quadraticBezierTo(bottomControlPoint.dx, bottomControlPoint.dy, corners.bottomLeft.dx, corners.bottomLeft.dy) - ..close(); - - canvas.drawPath(path, fillPaint); - textPainter.paint(canvas, textPosition); - } - - @override - bool shouldRepaint(covariant CustomPainter oldDelegate) { - return false; - } -} diff --git a/example/viam_example_app/lib/resources/arm_widgets/joint_positions_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/joint_positions_widget.dart deleted file mode 100644 index 058a90d8442..00000000000 --- a/example/viam_example_app/lib/resources/arm_widgets/joint_positions_widget.dart +++ /dev/null @@ -1,218 +0,0 @@ -import 'package:flutter/material.dart'; -import 'package:flutter/services.dart'; -import 'package:viam_sdk/viam_sdk.dart' as viam; - -class JointPositionsWidget extends StatefulWidget { - final viam.Arm arm; - const JointPositionsWidget({super.key, required this.arm}); - - @override - State createState() => _JointPositionsWidgetState(); -} - -class _JointPositionsWidgetState extends State { - List _startJointValues = []; - bool _isLive = false; - - @override - void initState() { - super.initState(); - _getJointInfo(); - } - - Future _getJointInfo() async { - // _startJointValues = await widget.arm.jointPositions(); - _startJointValues = List.generate(6, (index) { - return 0.0; - }); - setState(() {}); - } - - @override - Widget build(BuildContext context) { - return Column( - children: [ - Divider(), - Text( - 'Joint Angles', - style: TextStyle( - fontWeight: FontWeight.bold, - ), - ), - Divider(), - Padding( - padding: const EdgeInsets.all(20.0), - child: Column( - mainAxisSize: MainAxisSize.min, - children: _startJointValues.isEmpty - ? [CircularProgressIndicator.adaptive()] - : List.generate(_startJointValues.length, (index) { - return _BuildJointControlRow(index: index, arm: widget.arm, startJointValues: _startJointValues); - }), - ), - ), - Padding( - padding: const EdgeInsets.fromLTRB(20.0, 0, 20.0, 20.0), - child: Row( - spacing: 8, - children: [ - Switch( - value: _isLive, - activeColor: Colors.green, - inactiveTrackColor: Colors.transparent, - onChanged: (newValue) { - setState(() { - _isLive = newValue; - }); - }, - ), - Text( - "Live", - style: TextStyle(color: Colors.black), - ), - Spacer(), - OutlinedButtonTheme( - data: OutlinedButtonThemeData( - style: OutlinedButton.styleFrom( - foregroundColor: Colors.black, - iconColor: Colors.black, - overlayColor: Colors.grey, - shape: RoundedRectangleBorder(borderRadius: BorderRadius.all(Radius.circular(4)))), - ), - child: OutlinedButton.icon( - onPressed: _isLive ? null : () {}, - label: Text("Execute"), - icon: Icon(Icons.play_arrow), - ), - ) - ], - ), - ), - ], - ); - } -} - -class _BuildJointControlRow extends StatefulWidget { - final int index; - final viam.Arm arm; - final List startJointValues; - const _BuildJointControlRow({required this.index, required this.arm, required this.startJointValues}); - - @override - State<_BuildJointControlRow> createState() => _BuildJointControlRowState(); -} - -class _BuildJointControlRowState extends State<_BuildJointControlRow> { - static const double _minPosition = 0.0; - static const double _maxPosition = 180.0; - - List _jointValues = []; - List _textControllers = []; - - @override - void initState() { - _jointValues = widget.startJointValues; - _textControllers = List.generate( - _jointValues.length, - (index) => TextEditingController(text: _jointValues[index].toStringAsFixed(1)), - ); - super.initState(); - } - - @override - void dispose() { - for (final controller in _textControllers) { - controller.dispose(); - } - super.dispose(); - } - - void _updateJointValue(int index, double value) { - final clampedValue = value.clamp(_minPosition, _maxPosition); - - setState(() { - _jointValues[index] = clampedValue; - final formattedValue = clampedValue.toStringAsFixed(1); - if (_textControllers[index].text != formattedValue) { - _textControllers[index].text = formattedValue; - _textControllers[index].selection = TextSelection.fromPosition( - TextPosition(offset: _textControllers[index].text.length), - ); - } - }); - } - - @override - Widget build(BuildContext context) { - return Padding( - padding: const EdgeInsets.symmetric(vertical: 8.0), - child: Row( - children: [ - SizedBox( - width: 30, - child: Text( - 'J${widget.index + 1}', - style: Theme.of(context).textTheme.titleMedium, - ), - ), - Expanded( - child: SliderTheme( - data: SliderThemeData( - activeTrackColor: Colors.black, - inactiveTrackColor: Colors.grey, - thumbColor: Colors.black, - overlayColor: Colors.transparent, - showValueIndicator: ShowValueIndicator.never, - ), - child: Slider( - value: _jointValues[widget.index], - min: _minPosition, - max: _maxPosition, - divisions: (_maxPosition - _minPosition).toInt(), - onChanged: (newValue) => _updateJointValue(widget.index, newValue), - ), - ), - ), - SizedBox( - width: 70, - child: TextField( - controller: _textControllers[widget.index], - textAlign: TextAlign.center, - keyboardType: const TextInputType.numberWithOptions(decimal: true), - inputFormatters: [ - FilteringTextInputFormatter.allow(RegExp(r'^\d+\.?\d{0,1}')), - ], - style: const TextStyle(color: Colors.black), - cursorColor: Colors.black, - decoration: const InputDecoration( - border: OutlineInputBorder(), - focusedBorder: OutlineInputBorder( - borderSide: BorderSide(color: Colors.black), - ), - contentPadding: EdgeInsets.symmetric(horizontal: 8), - ), - onSubmitted: (newValue) { - final parsedValue = double.tryParse(newValue) ?? _jointValues[widget.index]; - _updateJointValue(widget.index, parsedValue); - }, - ), - ), - const SizedBox(width: 8), - IconButton( - icon: const Icon(Icons.remove), - onPressed: () { - _updateJointValue(widget.index, _jointValues[widget.index] - 1.0); - }, - ), - IconButton( - icon: const Icon(Icons.add), - onPressed: () { - _updateJointValue(widget.index, _jointValues[widget.index] + 1.0); - }, - ), - ], - ), - ); - } -} diff --git a/example/viam_example_app/lib/resources/arm_widgets/linear_arrows_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/linear_arrows_widget.dart deleted file mode 100644 index 17afa1a3434..00000000000 --- a/example/viam_example_app/lib/resources/arm_widgets/linear_arrows_widget.dart +++ /dev/null @@ -1,242 +0,0 @@ -import 'package:flutter/material.dart'; - -final size = 300.0; - -class LinearArrowsWidget extends StatelessWidget { - const LinearArrowsWidget({super.key}); - - @override - Widget build(BuildContext context) { - return Column( - children: [ - Divider(), - Text( - 'End-effector Position', - style: TextStyle( - fontWeight: FontWeight.bold, - ), - ), - Divider(), - Stack( - children: [ - _SlantedArrowPad( - // TODO: add functions for arrow functionality - onUp: () {}, - onDown: () {}, - onLeft: () {}, - onRight: () {}, - ), - _BuildCornerButton( - alignment: Alignment.topLeft, - direction: ArrowDirection.up, - label: 'Z+', - onPressed: () {}, - ), - _BuildCornerButton( - alignment: Alignment.topRight, - direction: ArrowDirection.down, - label: 'Z-', - onPressed: () {}, - ), - ], - ), - ], - ); - } -} - -class _BuildCornerButton extends StatelessWidget { - final Alignment alignment; - final ArrowDirection direction; - final String label; - final VoidCallback onPressed; - - const _BuildCornerButton({ - required this.alignment, - required this.direction, - required this.label, - required this.onPressed, - }); - - @override - Widget build(BuildContext context) { - return Align( - alignment: alignment, - child: Padding( - padding: const EdgeInsets.all(24.0), - child: SizedBox( - width: 100, - height: 100, - child: IconButton( - icon: Stack( - alignment: Alignment.center, - children: [ - CustomPaint( - painter: _LinearArrowPainter(direction: direction, color: Colors.black), - child: const SizedBox.expand(), - ), - Text( - label, - style: const TextStyle( - color: Colors.white, - fontWeight: FontWeight.bold, - fontSize: 16, - ), - ), - ], - ), - onPressed: onPressed, - ), - ), - ), - ); - } -} - -enum ArrowDirection { up, down, left, right } - -class _LinearArrowPainter extends CustomPainter { - final ArrowDirection direction; - final Color color; - - _LinearArrowPainter({required this.direction, required this.color}); - - @override - void paint(Canvas canvas, Size size) { - final paint = Paint() - ..color = color - ..style = PaintingStyle.fill; - - final path = Path(); - final w = size.width; - final h = size.height; - - switch (direction) { - case ArrowDirection.up: - path.moveTo(w / 2, 0); - path.lineTo(w, h * 0.6); - path.lineTo(w * 0.8, h * 0.6); - path.lineTo(w * 0.8, h); - path.lineTo(w * 0.2, h); - path.lineTo(w * 0.2, h * 0.6); - path.lineTo(0, h * 0.6); - break; - case ArrowDirection.down: - path.moveTo(w / 2, h); - path.lineTo(0, h * 0.4); - path.lineTo(w * 0.2, h * 0.4); - path.lineTo(w * 0.2, 0); - path.lineTo(w * 0.8, 0); - path.lineTo(w * 0.8, h * 0.4); - path.lineTo(w, h * 0.4); - break; - case ArrowDirection.left: - path.moveTo(0, h / 2); - path.lineTo(w * 0.6, 0); - path.lineTo(w * 0.6, h * 0.2); - path.lineTo(w, h * 0.2); - path.lineTo(w, h * 0.8); - path.lineTo(w * 0.6, h * 0.8); - path.lineTo(w * 0.6, h); - break; - case ArrowDirection.right: - path.moveTo(w, h / 2); - path.lineTo(w * 0.4, h); - path.lineTo(w * 0.4, h * 0.8); - path.lineTo(0, h * 0.8); - path.lineTo(0, h * 0.2); - path.lineTo(w * 0.4, h * 0.2); - path.lineTo(w * 0.4, 0); - break; - } - - path.close(); - canvas.drawPath(path, paint); - } - - @override - bool shouldRepaint(covariant _LinearArrowPainter oldDelegate) => false; -} - -class _SlantedArrowPad extends StatelessWidget { - final VoidCallback? onUp; - final VoidCallback? onDown; - final VoidCallback? onLeft; - final VoidCallback? onRight; - - const _SlantedArrowPad({ - this.onUp, - this.onDown, - this.onLeft, - this.onRight, - }); - - @override - Widget build(BuildContext context) { - return Center( - child: Transform( - transform: Matrix4.identity() - ..setEntry(3, 2, 0.0015) - ..rotateX(-0.9), - alignment: FractionalOffset.center, - child: SizedBox( - height: size, - width: size, - child: Stack( - children: [ - _BuildArrowButton(alignment: Alignment.topCenter, direction: ArrowDirection.up, onPressed: onUp, label: 'X-'), - _BuildArrowButton(alignment: Alignment.bottomCenter, direction: ArrowDirection.down, onPressed: onDown, label: 'X+'), - _BuildArrowButton(alignment: Alignment.centerLeft, direction: ArrowDirection.left, onPressed: onLeft, label: 'Y-'), - _BuildArrowButton(alignment: Alignment.centerRight, direction: ArrowDirection.right, onPressed: onRight, label: 'Y+'), - ], - ), - ), - ), - ); - } -} - -class _BuildArrowButton extends StatelessWidget { - final Alignment alignment; - final ArrowDirection direction; - final String label; - final VoidCallback? onPressed; - - const _BuildArrowButton({ - required this.alignment, - required this.direction, - required this.onPressed, - required this.label, - }); - - @override - Widget build(BuildContext context) { - return Align( - alignment: alignment, - child: SizedBox( - width: size / 2.5, - height: size / 2.5, - child: IconButton( - icon: Stack( - alignment: Alignment.center, - children: [ - CustomPaint( - painter: _LinearArrowPainter(direction: direction, color: Colors.black), - child: const SizedBox.expand(), - ), - Text( - label, - style: const TextStyle( - color: Colors.white, - fontWeight: FontWeight.bold, - fontSize: 16, - ), - ), - ], - ), - onPressed: onPressed, - ), - ), - ); - } -} diff --git a/example/viam_example_app/lib/resources/arm_widgets/orienation_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/orienation_widget.dart deleted file mode 100644 index 002c30280de..00000000000 --- a/example/viam_example_app/lib/resources/arm_widgets/orienation_widget.dart +++ /dev/null @@ -1,266 +0,0 @@ -import 'package:flutter/material.dart'; -import 'package:flutter/services.dart'; -import 'package:viam_sdk/viam_sdk.dart' as viam; - -class OrientationWidget extends StatefulWidget { - final viam.Arm arm; - const OrientationWidget({super.key, required this.arm}); - - @override - State createState() => _ArmControlWidgetState(); -} - -class _ArmControlWidgetState extends State { - static const double _minOrientation = -1.0; - static const double _maxOrientation = 1.0; - final _controlCount = 4; - bool _isLive = false; - - static const double _minTheta = -180; - static const double _maxTheta = 180; - List _controlValues = []; - - late final List _textControllers; - - @override - void initState() { - super.initState(); - _getStartOrientation(); - } - - Future _getStartOrientation() async { - final startPose = await widget.arm.endPosition(); - _controlValues = [startPose.oX, startPose.oY, startPose.oZ, startPose.theta]; - _textControllers = List.generate( - _controlCount, - (index) => TextEditingController(text: _controlValues[index].toStringAsFixed(1)), - ); - setState(() {}); - } - - @override - void dispose() { - for (final controller in _textControllers) { - controller.dispose(); - } - super.dispose(); - } - - void _updateControlValue(int index, double value) { - final clampedValue = value.clamp(_minOrientation, _maxOrientation); - - setState(() { - _controlValues[index] = clampedValue; - - final formattedValue = clampedValue.toStringAsFixed(1); - if (_textControllers[index].text != formattedValue) { - _textControllers[index].text = formattedValue; - _textControllers[index].selection = TextSelection.fromPosition( - TextPosition(offset: _textControllers[index].text.length), - ); - } - }); - } - - void _updateThetaValue(int index, double value) { - final clampedValue = value.clamp(_minTheta, _maxTheta); - setState(() { - _controlValues[index] = clampedValue; - - final formattedValue = clampedValue.toStringAsFixed(1); - if (_textControllers[index].text != formattedValue) { - _textControllers[index].text = formattedValue; - _textControllers[index].selection = TextSelection.fromPosition( - TextPosition(offset: _textControllers[index].text.length), - ); - } - }); - } - - @override - Widget build(BuildContext context) { - return Column( - children: [ - Divider(), - Text( - 'End-effector Orientation', - style: TextStyle( - fontWeight: FontWeight.bold, - ), - ), - Divider(), - Padding( - padding: const EdgeInsets.all(16.0), - child: _controlValues.isEmpty - ? CircularProgressIndicator.adaptive() - : Column( - mainAxisSize: MainAxisSize.min, - children: [ - _BuildJointControlRow( - label: 'OX', - value: _controlValues[0], - controller: _textControllers[0], - min: _minOrientation, - max: _maxOrientation, - onValueChanged: (newValue) => _updateControlValue(0, newValue), - ), - _BuildJointControlRow( - label: 'OY', - value: _controlValues[1], - controller: _textControllers[1], - min: _minOrientation, - max: _maxOrientation, - onValueChanged: (newValue) => _updateControlValue(1, newValue), - ), - _BuildJointControlRow( - label: 'OZ', - value: _controlValues[2], - controller: _textControllers[2], - min: _minOrientation, - max: _maxOrientation, - onValueChanged: (newValue) => _updateControlValue(2, newValue), - ), - _BuildJointControlRow( - label: 'Theta', - value: _controlValues[3], - controller: _textControllers[3], - min: _minTheta, - max: _maxTheta, - onValueChanged: (newValue) => _updateThetaValue(3, newValue), - ), - ], - ), - ), - Padding( - padding: const EdgeInsets.fromLTRB(20.0, 0, 20.0, 20.0), - child: Row( - spacing: 8, - children: [ - Switch( - value: _isLive, - activeColor: Colors.green, - inactiveTrackColor: Colors.transparent, - onChanged: (newValue) { - setState(() { - _isLive = newValue; - }); - }, - ), - Text( - "Live", - style: TextStyle(color: Colors.black), - ), - Spacer(), - OutlinedButtonTheme( - data: OutlinedButtonThemeData( - style: OutlinedButton.styleFrom( - foregroundColor: Colors.black, - iconColor: Colors.black, - overlayColor: Colors.grey, - shape: RoundedRectangleBorder(borderRadius: BorderRadius.all(Radius.circular(4)))), - ), - child: OutlinedButton.icon( - onPressed: _isLive ? null : () {}, - label: Text("Execute"), - icon: Icon(Icons.play_arrow), - ), - ) - ], - ), - ), - ], - ); - } -} - -class _BuildJointControlRow extends StatelessWidget { - final String label; - final double value; - final TextEditingController controller; - final double min; - final double max; - final ValueChanged onValueChanged; - - const _BuildJointControlRow({ - required this.label, - required this.value, - required this.controller, - required this.min, - required this.max, - required this.onValueChanged, - }); - - @override - Widget build(BuildContext context) { - return Padding( - padding: const EdgeInsets.symmetric(vertical: 8.0), - child: Row( - children: [ - // Control Label - SizedBox( - width: 70, - child: Text( - label, - style: Theme.of(context).textTheme.titleMedium, - ), - ), - Expanded( - child: SliderTheme( - data: SliderThemeData( - activeTrackColor: Colors.black, - inactiveTrackColor: Colors.grey, - thumbColor: Colors.black, - overlayColor: Colors.transparent, - showValueIndicator: ShowValueIndicator.never, - ), - child: Slider( - value: value, - min: min, - max: max, - divisions: ((max - min) * 10).toInt(), - label: value.toStringAsFixed(1), - onChanged: onValueChanged, - activeColor: Colors.black, - overlayColor: WidgetStateProperty.all(Colors.transparent), - ), - ), - ), - const SizedBox(width: 16), - SizedBox( - width: 70, - child: TextField( - controller: controller, - textAlign: TextAlign.center, - keyboardType: const TextInputType.numberWithOptions(decimal: true, signed: true), - inputFormatters: [ - FilteringTextInputFormatter.allow(RegExp(r'^-?\d+\.?\d{0,1}')), - ], - style: const TextStyle(color: Colors.black), - cursorColor: Colors.black, - decoration: const InputDecoration( - border: OutlineInputBorder(), - focusedBorder: OutlineInputBorder( - borderSide: BorderSide(color: Colors.black), - ), - contentPadding: EdgeInsets.symmetric(horizontal: 8), - ), - onSubmitted: (newValue) { - final parsedValue = double.tryParse(newValue) ?? value; - onValueChanged(parsedValue); - }, - ), - ), - const SizedBox(width: 8), - IconButton( - icon: const Icon(Icons.remove), - onPressed: () => onValueChanged(value - 0.1), - ), - IconButton( - icon: const Icon(Icons.add), - onPressed: () => onValueChanged(value + 0.1), - ), - ], - ), - ); - } -} diff --git a/example/viam_example_app/lib/resources/arm_widgets/position_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/position_widget.dart deleted file mode 100644 index a67e925b419..00000000000 --- a/example/viam_example_app/lib/resources/arm_widgets/position_widget.dart +++ /dev/null @@ -1,240 +0,0 @@ -import 'package:flutter/material.dart'; -import 'package:flutter/services.dart'; -import 'package:viam_sdk/viam_sdk.dart' as viam; - -class PositionWidget extends StatefulWidget { - final viam.Arm arm; - const PositionWidget({super.key, required this.arm}); - - @override - State createState() => _ArmControlWidgetState(); -} - -class _ArmControlWidgetState extends State { - static const double _minPosition = -10000; - static const double _maxPosition = 10000; - final _controlCount = 3; - bool _isLive = false; - - List _controlValues = []; - - late final List _textControllers; - - @override - void initState() { - super.initState(); - _getStartOrientation(); - } - - Future _getStartOrientation() async { - final startPose = await widget.arm.endPosition(); - _controlValues = [startPose.x, startPose.y, startPose.z]; - _textControllers = List.generate( - _controlCount, - (index) => TextEditingController(text: _controlValues[index].toStringAsFixed(1)), - ); - setState(() {}); - } - - @override - void dispose() { - for (final controller in _textControllers) { - controller.dispose(); - } - super.dispose(); - } - - void _updateControlValue(int index, double value) { - final clampedValue = value.clamp(_minPosition, _maxPosition); - - setState(() { - _controlValues[index] = clampedValue; - - final formattedValue = clampedValue.toStringAsFixed(1); - if (_textControllers[index].text != formattedValue) { - _textControllers[index].text = formattedValue; - _textControllers[index].selection = TextSelection.fromPosition( - TextPosition(offset: _textControllers[index].text.length), - ); - } - }); - } - - @override - Widget build(BuildContext context) { - return Column( - children: [ - Divider(), - Text( - 'End-effector Position', - style: TextStyle( - fontWeight: FontWeight.bold, - ), - ), - Divider(), - Padding( - padding: const EdgeInsets.all(20.0), - child: _controlValues.isEmpty - ? CircularProgressIndicator.adaptive() - : Column( - mainAxisSize: MainAxisSize.min, - children: [ - _BuildJointControlRow( - label: 'X', - value: _controlValues[0], - controller: _textControllers[0], - min: _minPosition, - max: _maxPosition, - onValueChanged: (newValue) => _updateControlValue(0, newValue), - ), - _BuildJointControlRow( - label: 'Y', - value: _controlValues[1], - controller: _textControllers[1], - min: _minPosition, - max: _maxPosition, - onValueChanged: (newValue) => _updateControlValue(1, newValue), - ), - _BuildJointControlRow( - label: 'Z', - value: _controlValues[2], - controller: _textControllers[2], - min: _minPosition, - max: _maxPosition, - onValueChanged: (newValue) => _updateControlValue(2, newValue), - ), - ], - ), - ), - Padding( - padding: const EdgeInsets.fromLTRB(20.0, 0, 20.0, 20.0), - child: Row( - spacing: 8, - children: [ - Switch( - value: _isLive, - activeColor: Colors.green, - inactiveTrackColor: Colors.transparent, - onChanged: (newValue) { - setState(() { - _isLive = newValue; - }); - }, - ), - Text( - "Live", - style: TextStyle(color: Colors.black), - ), - Spacer(), - OutlinedButtonTheme( - data: OutlinedButtonThemeData( - style: OutlinedButton.styleFrom( - foregroundColor: Colors.black, - iconColor: Colors.black, - overlayColor: Colors.grey, - shape: RoundedRectangleBorder(borderRadius: BorderRadius.all(Radius.circular(4)))), - ), - child: OutlinedButton.icon( - onPressed: _isLive ? null : () {}, - label: Text("Execute"), - icon: Icon(Icons.play_arrow), - ), - ) - ], - ), - ), - ], - ); - } -} - -class _BuildJointControlRow extends StatelessWidget { - final String label; - final double value; - final TextEditingController controller; - final double min; - final double max; - final ValueChanged onValueChanged; - - const _BuildJointControlRow({ - required this.label, - required this.value, - required this.controller, - required this.min, - required this.max, - required this.onValueChanged, - }); - - @override - Widget build(BuildContext context) { - return Padding( - padding: const EdgeInsets.symmetric(vertical: 8.0), - child: Row( - children: [ - SizedBox( - width: 30, - child: Text( - label, - style: Theme.of(context).textTheme.titleMedium, - ), - ), - Expanded( - child: SliderTheme( - data: SliderThemeData( - activeTrackColor: Colors.black, - inactiveTrackColor: Colors.grey, - thumbColor: Colors.black, - overlayColor: Colors.transparent, - showValueIndicator: ShowValueIndicator.never, - ), - child: Slider( - value: value, - min: min, - max: max, - divisions: ((max - min) * 10).toInt(), - label: value.toStringAsFixed(1), - onChanged: onValueChanged, - activeColor: Colors.black, - overlayColor: WidgetStateProperty.all(Colors.transparent), - ), - ), - ), - const SizedBox(width: 16), - SizedBox( - width: 70, - child: TextField( - controller: controller, - textAlign: TextAlign.center, - keyboardType: const TextInputType.numberWithOptions(decimal: true, signed: true), - inputFormatters: [ - FilteringTextInputFormatter.allow(RegExp(r'^-?\d+\.?\d{0,1}')), - ], - style: const TextStyle(color: Colors.black), - cursorColor: Colors.black, - decoration: const InputDecoration( - border: OutlineInputBorder(), - focusedBorder: OutlineInputBorder( - borderSide: BorderSide(color: Colors.black), - ), - contentPadding: EdgeInsets.symmetric(horizontal: 8), - ), - onSubmitted: (newValue) { - final parsedValue = double.tryParse(newValue) ?? value; - onValueChanged(parsedValue); - }, - ), - ), - const SizedBox(width: 8), - IconButton( - icon: const Icon(Icons.remove), - onPressed: () => onValueChanged(value - 0.1), - ), - IconButton( - icon: const Icon(Icons.add), - onPressed: () => onValueChanged(value + 0.1), - ), - ], - ), - ); - } -} diff --git a/example/viam_example_app/pubspec.yaml b/example/viam_example_app/pubspec.yaml index bb3ad2ca564..60f36c581c2 100644 --- a/example/viam_example_app/pubspec.yaml +++ b/example/viam_example_app/pubspec.yaml @@ -11,6 +11,7 @@ dependencies: sdk: flutter flutter_dotenv: ^5.1.0 sensors_plus: ^7.0.0 + vector_math: ^2.2.0 viam_sdk: path: ../../ From 7a50d503fed15af97e39a0ea9b34ffe1bedb5593 Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Wed, 5 Nov 2025 17:08:25 -0500 Subject: [PATCH 09/27] arm screen --- example/viam_example_app/lib/resources/arm_screen.dart | 2 -- 1 file changed, 2 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_screen.dart b/example/viam_example_app/lib/resources/arm_screen.dart index 2ef364a4454..1e60fcacb76 100644 --- a/example/viam_example_app/lib/resources/arm_screen.dart +++ b/example/viam_example_app/lib/resources/arm_screen.dart @@ -21,8 +21,6 @@ class ViamArmWidgetNew extends StatelessWidget { ); } } - -// need real arm notifier from martha class ArmNotifier extends ChangeNotifier { ArmNotifier(); From 8d5e16d7294f211c308026d264fe01cdf8a16275 Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Wed, 5 Nov 2025 17:08:49 -0500 Subject: [PATCH 10/27] formatting --- example/viam_example_app/lib/resources/arm_screen.dart | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example/viam_example_app/lib/resources/arm_screen.dart b/example/viam_example_app/lib/resources/arm_screen.dart index 1e60fcacb76..e064d357d19 100644 --- a/example/viam_example_app/lib/resources/arm_screen.dart +++ b/example/viam_example_app/lib/resources/arm_screen.dart @@ -27,4 +27,4 @@ class ArmNotifier extends ChangeNotifier { void update() { notifyListeners(); } -} \ No newline at end of file +} From 4fe9c362b39bf1127ad3d5f25817215f303a5322 Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Wed, 5 Nov 2025 17:11:29 -0500 Subject: [PATCH 11/27] remove changes to robot screen --- example/viam_example_app/lib/robot_screen.dart | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/example/viam_example_app/lib/robot_screen.dart b/example/viam_example_app/lib/robot_screen.dart index 3a0df80be7d..4582e308d84 100644 --- a/example/viam_example_app/lib/robot_screen.dart +++ b/example/viam_example_app/lib/robot_screen.dart @@ -60,10 +60,7 @@ class _RobotScreenState extends State { // Using the authenticated [Viam] the received as a parameter, // we can obtain a connection to the Robot. // There is a helpful convenience method on the [Viam] instance for this. - // final robotClient = await widget._viam.getRobotClient(widget.robot); - final options = RobotClientOptions.withApiKey(dotenv.env['API_KEY_ID']!, dotenv.env['API_KEY']!); - options.dialOptions.attemptMdns = false; - final robotClient = await RobotClient.atAddress(dotenv.env['ROBOT_LOCATION']!, options); + final robotClient = await widget._viam.getRobotClient(widget.robot); setState(() { client = robotClient; _isLoading = false; From b308b41e53442c847347b63af3a5bf62eed9bd8a Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Wed, 5 Nov 2025 17:12:34 -0500 Subject: [PATCH 12/27] connect to robot client mdns false --- example/viam_example_app/lib/robot_screen.dart | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/example/viam_example_app/lib/robot_screen.dart b/example/viam_example_app/lib/robot_screen.dart index 4582e308d84..0f0230070e4 100644 --- a/example/viam_example_app/lib/robot_screen.dart +++ b/example/viam_example_app/lib/robot_screen.dart @@ -60,7 +60,9 @@ class _RobotScreenState extends State { // Using the authenticated [Viam] the received as a parameter, // we can obtain a connection to the Robot. // There is a helpful convenience method on the [Viam] instance for this. - final robotClient = await widget._viam.getRobotClient(widget.robot); + final options = RobotClientOptions.withApiKey(dotenv.env['API_KEY_ID']!, dotenv.env['API_KEY']!); + options.dialOptions.attemptMdns = false; + final robotClient = await RobotClient.atAddress(dotenv.env['ROBOT_LOCATION']!, options); setState(() { client = robotClient; _isLoading = false; From 58a2312dcd8c6fc71f5eee37f04d08fdf5949dde Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Wed, 5 Nov 2025 17:27:20 -0500 Subject: [PATCH 13/27] queue --- .../lib/resources/arm_widgets/imu_widget.dart | 94 +++++++------------ 1 file changed, 32 insertions(+), 62 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index c829ea94240..c8b1bb020a0 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -32,9 +32,9 @@ class _ImuWidgetState extends State { // Arm movement control variables DateTime? _lastArmCommandTime; - static const Duration _armCommandInterval = Duration(milliseconds: 200); // Rate limit: 10 commands/sec + static const Duration _armCommandInterval = Duration(milliseconds: 200); // Rate limit: 5 commands/sec - // Scale factor: 300mm/meter = 300mm of arm movement per meter of phone movement + // Scale factor: 1000mm/meter = 1000mm of arm movement per meter of phone movement static const double _positionScale = 1000.0; static const double _velocityDecay = 0.90; // Decay factor to prevent drift @@ -58,9 +58,9 @@ class _ImuWidgetState extends State { DateTime? _lastIntegrationTime; // Orientation (radians) - double _orientationX = 0.0; // Roll (rotation around X-axis, in radians) - double _orientationY = 0.0; // Pitch (rotation around Y-axis, in radians) - double _orientationZ = 0.0; // Yaw (rotation around Z-axis, in radians) + double _orientationX = 0.0; // Roll + double _orientationY = 0.0; // Pitch + double _orientationZ = 0.0; // Yaw DateTime? _lastGyroIntegrationTime; // Arm position in the world when we set the reference. set once when you press "set reference" and stays constant. @@ -73,8 +73,7 @@ class _ImuWidgetState extends State { _streamSubscriptions.add( userAccelerometerEventStream(samplingPeriod: sensorInterval).listen( (UserAccelerometerEvent event) { - // Move arm based on accelerometer data - _moveArmFromImu(event); + _createPoseFromImu(event); }, onError: (e) { showDialog( @@ -152,10 +151,17 @@ class _ImuWidgetState extends State { _orientationZ *= 0.999; } - /// Move the arm based on IMU accelerometer data using acceleration to get position - /// we do this by taking integral of acceleration to get velocity, and then integral of velocity to get position. - Future _moveArmFromImu(UserAccelerometerEvent event) async { - // event is the accelerometer data from the phone ^^ + /// Create a pose based on IMU accelerometer data using acceleration to get position + /// The pose is added to a queue for sequential processing. + Future _createPoseFromImu(UserAccelerometerEvent event) async { + if (!_isReferenceSet || _referenceArmPose == null) { + return; + } + + if (_isMovingArm) { + return; + } + final now = DateTime.now(); // Initialize integration time on first run @@ -165,88 +171,59 @@ class _ImuWidgetState extends State { } // Calculate time delta between now and last integration time (in seconds) - // why? b/c we need to know how much time has passed, aka how long have we been accelerating for? // tldr: to get velocity, we need acceleration * time. final dt = now.difference(_lastIntegrationTime!).inMilliseconds / 1000.0; _lastIntegrationTime = now; // Skip if dt is too large, meaning the phone has been stationary for too long in between movements. // if (dt > 0.5) { - // // this number might be too small // return; // } - // Apply dead zone to acceleration - // peoples hands are shaky, phone sensors are jittery, this helps filter out the noise. + // Apply dead zone to acceleration to filter out noise final accelX = event.x.abs() > _deadZone ? event.x : 0.0; final accelY = event.y.abs() > _deadZone ? event.y : 0.0; final accelZ = event.z.abs() > _deadZone ? event.z : 0.0; - // print('eventz: ${event.z}'); if (accelX == 0.0 && accelY == 0.0 && accelZ == 0.0) { _velocityX = 0.0; _velocityY = 0.0; _velocityZ = 0.0; - // return; } - // STEP 1: Calculate velocity. velocity is the integral of acceleration wrt time. (v = v0 + a*dt) + // Calculate velocity _velocityX += accelX * dt; _velocityY += accelY * dt; _velocityZ += accelZ * dt; // Apply decay to prevent drift when stationary - // we are multiply velocity by 0.95 so that we reduce it by 5%. _velocityX *= _velocityDecay; _velocityY *= _velocityDecay; _velocityZ *= _velocityDecay; - // STEP 2: Caluclate position. position is the integral of velocity wrt time. (p = p0 + v*dt) + // Calculate position _positionX += _velocityX * dt; - // print('positionX: $_positionX'); _positionY += _velocityY * dt; - // print('positionY: $_positionY'); _positionZ += _velocityZ * dt; - print('positionZ: $_positionZ'); // Rate limiting, don't send arm commands too frequently. - // this prevents us from sending too many commands to the arm too quickly. if (_lastArmCommandTime != null) { final timeSinceLastCommand = now.difference(_lastArmCommandTime!); if (timeSinceLastCommand < _armCommandInterval) { - return; // Too soon, skip this update + return; } } - // Don't send new command if previous one is still executing - if (_isMovingArm) { - return; - } - - // // Don't move arm if reference point hasn't been set - if (!_isReferenceSet || _referenceArmPose == null) { - return; - } - - // ready to move the arm! - // mark that we are about to send a command. - // save the current time (for rate limiting next time) - // set the "busy" flag to true + // Ready to create and queue the pose _lastArmCommandTime = now; _isMovingArm = true; try { - // 3. Calculate new target position based on reference + phone displacement - // Reference arm position + (phone displacement * scale factor) - // remeber: referenceArmPose is the arm's position in the real world when we set the reference. - // positionScale = 50mm/meter, so if we move the phone 1 meter, the arm will move 50mm. - final newX = _referenceArmPose!.x + (_positionY * _positionScale); // flipped x and y - final newY = (_referenceArmPose!.y + (_positionX * _positionScale)) * -1; // flipped y - final newZ = (_referenceArmPose!.z + (_positionZ * _positionScale)); // flipped z - - // TODO: convert euler angles to orientation vector using spatial math package - - // final quaternion = vector_math.Quaternion.euler(_orientationZ, _orientationY, _orientationX); // Yaw, Pitch, Roll - // final quaternion = math.Quaternion(_orientationZ, _orientationY, _orientationX, 0.0); + // Calculate new target position based on reference + phone displacement + // referenceArmPose is the arm's position in the real world when we set the reference + final newX = _referenceArmPose!.x + (_positionY * _positionScale); + final newY = (_referenceArmPose!.y + (_positionX * _positionScale)) * -1; + final newZ = (_referenceArmPose!.z + (_positionZ * _positionScale)); + final quaternion = vector_math.Quaternion.identity(); quaternion.setEuler(_orientationZ, _orientationY, _orientationX); // Yaw, Pitch, Roll final orientationVector = quatToOV(quaternion); @@ -254,17 +231,11 @@ class _ImuWidgetState extends State { final newOrientationY = _referenceArmPose!.oY + orientationVector.y; final newOrientationZ = _referenceArmPose!.oZ + orientationVector.z; - // 4. Create new pose with position and orientation - // positions are what we calculated right above - // orientations are calculated from the reference plus the orientation changes from the gyroscope. final newPose = Pose( - x: newX, // flipped x and y + x: newX, y: newY, z: newZ, theta: _referenceArmPose!.theta, // Keep theta from reference - // oX: _referenceArmPose!.oX + _orientationX, // these are probs wrong bc we are adding orientation vector values + ueler values - // oY: _referenceArmPose!.oY + _orientationY, - // oZ: _referenceArmPose!.oZ + _orientationZ, oX: newOrientationX, oY: newOrientationY, oZ: newOrientationZ, @@ -288,7 +259,6 @@ class _ImuWidgetState extends State { _processQueueSequentially(); } } catch (e) { - // Handle errors gracefully setState(() { _lastError = e.toString(); }); @@ -297,13 +267,13 @@ class _ImuWidgetState extends State { } } - /// Process pose queue sequentially, executing every 5th pose + /// Process pose queue sequentially, executing every 10th pose Future _processQueueSequentially() async { _isProcessingQueue = true; while (_poseQueue.isNotEmpty) { - // Only execute every 5th pose - if (_poseCounter % 5 == 0) { + // Only execute every 10th pose + if (_poseCounter % 10 == 0) { // Get the latest pose from the queue (skip intermediate ones) final poseToExecute = _poseQueue.last; _poseQueue.clear(); // Clear all accumulated poses From a4d0fb25c0b38a4937717ca47f087da71ca4bd9f Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Wed, 5 Nov 2025 17:34:29 -0500 Subject: [PATCH 14/27] clean up a little --- .../lib/resources/arm_widgets/imu_widget.dart | 60 +++++++------------ 1 file changed, 23 insertions(+), 37 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index c8b1bb020a0..1489a0edd87 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -30,11 +30,10 @@ class _ImuWidgetState extends State { final _streamSubscriptions = >[]; Duration sensorInterval = SensorInterval.normalInterval; - // Arm movement control variables DateTime? _lastArmCommandTime; - static const Duration _armCommandInterval = Duration(milliseconds: 200); // Rate limit: 5 commands/sec - - // Scale factor: 1000mm/meter = 1000mm of arm movement per meter of phone movement + // Rate limit: 5 commands/sec + static const Duration _armCommandInterval = Duration(milliseconds: 200); + // Scale factor: 1000mm/meter for 1:1 scale static const double _positionScale = 1000.0; static const double _velocityDecay = 0.90; // Decay factor to prevent drift @@ -69,47 +68,34 @@ class _ImuWidgetState extends State { // the latest postion of the arm, updates everytime the arm moves. used to display the arm's position. Pose? _currentArmPose; - _initAccelerometer() { + void _initAccelerometer() { _streamSubscriptions.add( userAccelerometerEventStream(samplingPeriod: sensorInterval).listen( - (UserAccelerometerEvent event) { - _createPoseFromImu(event); - }, - onError: (e) { - showDialog( - context: context, - builder: (context) { - return const AlertDialog( - title: Text("Sensor Not Found"), - content: Text("It seems that your device doesn't support User Accelerometer Sensor"), - ); - }); - }, + _createPoseFromImu, + onError: (_) => _showSensorError("User Accelerometer"), cancelOnError: true, ), ); _streamSubscriptions.add( gyroscopeEventStream(samplingPeriod: sensorInterval).listen( - (GyroscopeEvent event) { - // Update orientation based on gyroscope data - _updateOrientationFromGyroscope(event); - }, - onError: (e) { - showDialog( - context: context, - builder: (context) { - return const AlertDialog( - title: Text("Sensor Not Found"), - content: Text("It seems that your device doesn't support Gyroscope Sensor"), - ); - }); - }, + _updateOrientationFromGyroscope, + onError: (_) => _showSensorError("Gyroscope"), cancelOnError: true, ), ); } + void _showSensorError(String sensorName) { + showDialog( + context: context, + builder: (context) => AlertDialog( + title: const Text("Sensor Not Found"), + content: Text("It seems that your device doesn't support $sensorName Sensor"), + ), + ); + } + /// Update orientation by integrating gyroscope angular velocity void _updateOrientationFromGyroscope(GyroscopeEvent event) { final now = DateTime.now(); @@ -135,12 +121,12 @@ class _ImuWidgetState extends State { return; } - // Calucluate orientation change: integrate angular velocity over time to get orientation (angle). + // Calucluate orientation change: integrate angular velocity over time to get orientation. // Gyroscope values are in radians/second // event.x = rotation rate around X-axis (roll) // event.y = rotation rate around Y-axis (pitch) // event.z = rotation rate around Z-axis (yaw) - _orientationX += event.x * dt; + _orientationX += event.x * dt; _orientationY += event.y * dt; _orientationZ += event.z * dt; @@ -176,9 +162,9 @@ class _ImuWidgetState extends State { _lastIntegrationTime = now; // Skip if dt is too large, meaning the phone has been stationary for too long in between movements. - // if (dt > 0.5) { - // return; - // } + if (dt > 0.5) { + return; + } // Apply dead zone to acceleration to filter out noise final accelX = event.x.abs() > _deadZone ? event.x : 0.0; From 5ec98eb403edb906c4de042d04442439e9fe6e62 Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Wed, 5 Nov 2025 17:42:11 -0500 Subject: [PATCH 15/27] every 20th pose --- .../lib/resources/arm_widgets/imu_widget.dart | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index 1489a0edd87..debe5987e83 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -258,8 +258,8 @@ class _ImuWidgetState extends State { _isProcessingQueue = true; while (_poseQueue.isNotEmpty) { - // Only execute every 10th pose - if (_poseCounter % 10 == 0) { + // Only execute every 20th pose + if (_poseCounter % 20 == 0) { // Get the latest pose from the queue (skip intermediate ones) final poseToExecute = _poseQueue.last; _poseQueue.clear(); // Clear all accumulated poses From fd3f454c7e7e5615c00fd89d3ac41aff2426c4c4 Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Wed, 5 Nov 2025 17:52:52 -0500 Subject: [PATCH 16/27] add target and current position --- .../lib/resources/arm_widgets/imu_widget.dart | 73 +++++++++++-------- 1 file changed, 42 insertions(+), 31 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index debe5987e83..2b03bb90398 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -41,7 +41,7 @@ class _ImuWidgetState extends State { bool _isMovingArm = false; String? _lastError; - // Pose queue for batching arm movements + // Pose queue for batching arm movements to reduce lag final List _poseQueue = []; int _poseCounter = 0; bool _isProcessingQueue = false; @@ -62,10 +62,9 @@ class _ImuWidgetState extends State { double _orientationZ = 0.0; // Yaw DateTime? _lastGyroIntegrationTime; - // Arm position in the world when we set the reference. set once when you press "set reference" and stays constant. - Pose? _referenceArmPose; + Pose? _referenceArmPose; // arm position set once when you press "set reference" bool _isReferenceSet = false; - // the latest postion of the arm, updates everytime the arm moves. used to display the arm's position. + Pose? _targetArmPose; Pose? _currentArmPose; void _initAccelerometer() { @@ -126,12 +125,11 @@ class _ImuWidgetState extends State { // event.x = rotation rate around X-axis (roll) // event.y = rotation rate around Y-axis (pitch) // event.z = rotation rate around Z-axis (yaw) - _orientationX += event.x * dt; + _orientationX += event.x * dt; _orientationY += event.y * dt; _orientationZ += event.z * dt; // Apply small decay to prevent drift - // the 0.999 is aritrary for now _orientationX *= 0.999; _orientationY *= 0.999; _orientationZ *= 0.999; @@ -235,6 +233,9 @@ class _ImuWidgetState extends State { newPose.oZ == _currentArmPose!.oZ) { return; } + setState(() { + _targetArmPose = newPose; + }); // Add pose to queue _poseQueue.add(newPose); @@ -253,7 +254,9 @@ class _ImuWidgetState extends State { } } - /// Process pose queue sequentially, executing every 10th pose + /// Process pose queue sequentially, executing every 20th pose + /// Instead of sending every pose immediately (which causes lag), we queue them + /// and only execute every Nth pose, using the latest position from the queue Future _processQueueSequentially() async { _isProcessingQueue = true; @@ -268,7 +271,7 @@ class _ImuWidgetState extends State { await widget.arm.moveToPosition(poseToExecute); setState(() { _lastError = null; - _currentArmPose = poseToExecute; // Store for display + _currentArmPose = poseToExecute; }); } catch (e) { setState(() { @@ -317,7 +320,8 @@ class _ImuWidgetState extends State { // Store the current arm position as the reference _referenceArmPose = currentArmPose; - _currentArmPose = currentArmPose; // Also store for display + _targetArmPose = currentArmPose; // Initialize target pose + _currentArmPose = currentArmPose; // Initialize current pose _isReferenceSet = true; _lastError = null; }); @@ -333,19 +337,28 @@ class _ImuWidgetState extends State { return Column( mainAxisAlignment: MainAxisAlignment.center, children: [ - const Text("Arm Position (Real World)", style: TextStyle(fontWeight: FontWeight.bold, fontSize: 18)), + const Text("Target Position", style: TextStyle(fontWeight: FontWeight.bold, fontSize: 18, color: Colors.blue)), + const SizedBox(height: 10), + if (_targetArmPose != null) ...[ + Text("X: ${_targetArmPose!.x.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + Text("Y: ${_targetArmPose!.y.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + Text("Z: ${_targetArmPose!.z.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + ] else + const Text("No target yet", style: TextStyle(fontSize: 12, color: Colors.grey)), const SizedBox(height: 20), + const Text("Current Position", style: TextStyle(fontWeight: FontWeight.bold, fontSize: 18, color: Colors.green)), + const SizedBox(height: 10), if (_currentArmPose != null) ...[ - Text("X: ${_currentArmPose!.x.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 16)), - Text("Y: ${_currentArmPose!.y.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 16)), - Text("Z: ${_currentArmPose!.z.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 16)), - const SizedBox(height: 15), - const Text("Orientation:", style: TextStyle(fontWeight: FontWeight.bold, fontSize: 16)), - Text("oX (Roll): ${_currentArmPose!.oX.toStringAsFixed(3)} rad", style: const TextStyle(fontSize: 14)), - Text("oY (Pitch): ${_currentArmPose!.oY.toStringAsFixed(3)} rad", style: const TextStyle(fontSize: 14)), - Text("oZ (Yaw): ${_currentArmPose!.oZ.toStringAsFixed(3)} rad", style: const TextStyle(fontSize: 14)), + Text("X: ${_currentArmPose!.x.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + Text("Y: ${_currentArmPose!.y.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + Text("Z: ${_currentArmPose!.z.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + const SizedBox(height: 10), + const Text("Orientation:", style: TextStyle(fontWeight: FontWeight.bold, fontSize: 14)), + Text("oX (Roll): ${_currentArmPose!.oX.toStringAsFixed(3)} rad", style: const TextStyle(fontSize: 12)), + Text("oY (Pitch): ${_currentArmPose!.oY.toStringAsFixed(3)} rad", style: const TextStyle(fontSize: 12)), + Text("oZ (Yaw): ${_currentArmPose!.oZ.toStringAsFixed(3)} rad", style: const TextStyle(fontSize: 12)), ] else - const Text("No position data yet", style: TextStyle(fontSize: 14, color: Colors.grey)), + const Text("No position data yet", style: TextStyle(fontSize: 12, color: Colors.grey)), const SizedBox(height: 30), ElevatedButton( onPressed: _setReference, @@ -380,20 +393,9 @@ class _ImuWidgetState extends State { ); } - @override - void dispose() { - super.dispose(); - for (final subscription in _streamSubscriptions) { - subscription.cancel(); - } - // Clear pose queue on dispose - _poseQueue.clear(); - } - /// Converts a unit quaternion (q) to an OrientationVector. /// - /// q: The input rotation quaternion. (Dart: (x, y, z, w) = (Imag, Jmag, Kmag, Real)) - /// Converted from go code to flutter using gemini + /// Converted from go code to flutter using gemin Orientation_OrientationVectorRadians quatToOV(vector_math.Quaternion q) { double orientationVectorPoleRadius = 0.0001; double defaultAngleEpsilon = 1e-4; @@ -490,4 +492,13 @@ class _ImuWidgetState extends State { return ov; } + + @override + void dispose() { + super.dispose(); + for (final subscription in _streamSubscriptions) { + subscription.cancel(); + } + _poseQueue.clear(); + } } From 404761b13fde47d4b93803e862980c6cad1329a7 Mon Sep 17 00:00:00 2001 From: martha-johnston Date: Thu, 6 Nov 2025 15:51:51 +0200 Subject: [PATCH 17/27] reformat display positions --- .../lib/resources/arm_widgets/imu_widget.dart | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index 2b03bb90398..2dea63d92e5 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -343,6 +343,10 @@ class _ImuWidgetState extends State { Text("X: ${_targetArmPose!.x.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), Text("Y: ${_targetArmPose!.y.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), Text("Z: ${_targetArmPose!.z.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + Text("oX: ${_targetArmPose!.oX.toStringAsFixed(2)} rad", style: const TextStyle(fontSize: 14)), + Text("oY: ${_targetArmPose!.oY.toStringAsFixed(2)} rad", style: const TextStyle(fontSize: 14)), + Text("oZ: ${_targetArmPose!.oZ.toStringAsFixed(2)} rad", style: const TextStyle(fontSize: 14)), + Text("Theta: ${_targetArmPose!.theta.toStringAsFixed(1)} rad", style: const TextStyle(fontSize: 14)), ] else const Text("No target yet", style: TextStyle(fontSize: 12, color: Colors.grey)), const SizedBox(height: 20), @@ -352,11 +356,10 @@ class _ImuWidgetState extends State { Text("X: ${_currentArmPose!.x.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), Text("Y: ${_currentArmPose!.y.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), Text("Z: ${_currentArmPose!.z.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), - const SizedBox(height: 10), - const Text("Orientation:", style: TextStyle(fontWeight: FontWeight.bold, fontSize: 14)), - Text("oX (Roll): ${_currentArmPose!.oX.toStringAsFixed(3)} rad", style: const TextStyle(fontSize: 12)), - Text("oY (Pitch): ${_currentArmPose!.oY.toStringAsFixed(3)} rad", style: const TextStyle(fontSize: 12)), - Text("oZ (Yaw): ${_currentArmPose!.oZ.toStringAsFixed(3)} rad", style: const TextStyle(fontSize: 12)), + Text("oX: ${_currentArmPose!.oX.toStringAsFixed(2)} rad", style: const TextStyle(fontSize: 14)), + Text("oY: ${_currentArmPose!.oY.toStringAsFixed(2)} rad", style: const TextStyle(fontSize: 14)), + Text("oZ: ${_currentArmPose!.oZ.toStringAsFixed(2)} rad", style: const TextStyle(fontSize: 14)), + Text("Theta: ${_currentArmPose!.theta.toStringAsFixed(1)} rad", style: const TextStyle(fontSize: 14)), ] else const Text("No position data yet", style: TextStyle(fontSize: 12, color: Colors.grey)), const SizedBox(height: 30), From 6d33cbab31053ddbe0c1a91cde8849d4056e818a Mon Sep 17 00:00:00 2001 From: martha-johnston Date: Thu, 6 Nov 2025 16:14:19 +0200 Subject: [PATCH 18/27] add execute button --- .../lib/resources/arm_widgets/imu_widget.dart | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index 2dea63d92e5..1fac1bc479a 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -23,7 +23,7 @@ class ImuWidget extends StatefulWidget { class _ImuWidgetState extends State { @override void initState() { - _initAccelerometer(); + _initImu(); super.initState(); } @@ -67,7 +67,7 @@ class _ImuWidgetState extends State { Pose? _targetArmPose; Pose? _currentArmPose; - void _initAccelerometer() { + void _initImu() { _streamSubscriptions.add( userAccelerometerEventStream(samplingPeriod: sensorInterval).listen( _createPoseFromImu, @@ -262,7 +262,7 @@ class _ImuWidgetState extends State { while (_poseQueue.isNotEmpty) { // Only execute every 20th pose - if (_poseCounter % 20 == 0) { + if (_poseCounter % 5 == 0) { // Get the latest pose from the queue (skip intermediate ones) final poseToExecute = _poseQueue.last; _poseQueue.clear(); // Clear all accumulated poses @@ -392,6 +392,7 @@ class _ImuWidgetState extends State { "Move your phone through space!", style: TextStyle(fontSize: 12, fontStyle: FontStyle.italic), ), + TextButton(onPressed: () async => await widget.arm.moveToPosition(_targetArmPose!), child: Text("Execute")) ], ); } From e2b2f6c2a95eb3c076bef47eec27e2a55f4e6372 Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Thu, 6 Nov 2025 12:06:53 -0500 Subject: [PATCH 19/27] added deadzones per axis --- .../lib/resources/arm_widgets/imu_widget.dart | 80 ++++++++++++------- 1 file changed, 50 insertions(+), 30 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index 1fac1bc479a..aec70e34672 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -28,16 +28,18 @@ class _ImuWidgetState extends State { } final _streamSubscriptions = >[]; - Duration sensorInterval = SensorInterval.normalInterval; + Duration sensorInterval = SensorInterval.uiInterval; DateTime? _lastArmCommandTime; // Rate limit: 5 commands/sec static const Duration _armCommandInterval = Duration(milliseconds: 200); - // Scale factor: 1000mm/meter for 1:1 scale - static const double _positionScale = 1000.0; + static const double _positionScale = 300.0; - static const double _velocityDecay = 0.90; // Decay factor to prevent drift - static const double _deadZone = 0.3; // Ignore small accelerations + static const double _velocityDecay = 0.99; + + static const double _deadZoneZ = 0.3; + static const double _deadZoneXY = 0.1; + static const double _velocityThreshold = 0.01; // Threshold below which velocity is considered zero bool _isMovingArm = false; String? _lastError; @@ -165,13 +167,20 @@ class _ImuWidgetState extends State { } // Apply dead zone to acceleration to filter out noise - final accelX = event.x.abs() > _deadZone ? event.x : 0.0; - final accelY = event.y.abs() > _deadZone ? event.y : 0.0; - final accelZ = event.z.abs() > _deadZone ? event.z : 0.0; + final accelX = event.x.abs() > _deadZoneXY ? event.x : 0.0; + final accelY = event.y.abs() > _deadZoneXY ? event.y : 0.0; + final accelZ = event.z.abs() > _deadZoneZ ? event.z : 0.0; + print("event.x: ${event.x}, event.y: ${event.y}, event.z: ${event.z}"); + + // final accelX = event.x; + // final accelY = event.y; + // final accelZ = event.z; if (accelX == 0.0 && accelY == 0.0 && accelZ == 0.0) { + // Phone is stationary, reset velocity to prevent drift from sensor noise _velocityX = 0.0; _velocityY = 0.0; _velocityZ = 0.0; + return; // Don't update position } // Calculate velocity @@ -184,19 +193,16 @@ class _ImuWidgetState extends State { _velocityY *= _velocityDecay; _velocityZ *= _velocityDecay; + // Clamp very small velocities to zero to prevent creep + if (_velocityX.abs() < _velocityThreshold) _velocityX = 0.0; + if (_velocityY.abs() < _velocityThreshold) _velocityY = 0.0; + if (_velocityZ.abs() < _velocityThreshold) _velocityZ = 0.0; + // Calculate position _positionX += _velocityX * dt; _positionY += _velocityY * dt; _positionZ += _velocityZ * dt; - // Rate limiting, don't send arm commands too frequently. - if (_lastArmCommandTime != null) { - final timeSinceLastCommand = now.difference(_lastArmCommandTime!); - if (timeSinceLastCommand < _armCommandInterval) { - return; - } - } - // Ready to create and queue the pose _lastArmCommandTime = now; _isMovingArm = true; @@ -204,9 +210,9 @@ class _ImuWidgetState extends State { try { // Calculate new target position based on reference + phone displacement // referenceArmPose is the arm's position in the real world when we set the reference - final newX = _referenceArmPose!.x + (_positionY * _positionScale); - final newY = (_referenceArmPose!.y + (_positionX * _positionScale)) * -1; - final newZ = (_referenceArmPose!.z + (_positionZ * _positionScale)); + final newX = _referenceArmPose!.x + (_positionX * _positionScale); + final newY = _referenceArmPose!.y + (_positionY * _positionScale); + final newZ = _referenceArmPose!.z + (_positionZ * _positionScale); final quaternion = vector_math.Quaternion.identity(); quaternion.setEuler(_orientationZ, _orientationY, _orientationX); // Yaw, Pitch, Roll @@ -225,13 +231,15 @@ class _ImuWidgetState extends State { oZ: newOrientationZ, ); - if (newPose.x == _currentArmPose!.x && - newPose.y == _currentArmPose!.y && - newPose.z == _currentArmPose!.z && - newPose.oX == _currentArmPose!.oX && - newPose.oY == _currentArmPose!.oY && - newPose.oZ == _currentArmPose!.oZ) { - return; + if (_poseQueue.isNotEmpty) { + if (newPose.x == _poseQueue.last.x && + newPose.y == _poseQueue.last.y && + newPose.z == _poseQueue.last.z && + newPose.oX == _poseQueue.last.oX && + newPose.oY == _poseQueue.last.oY && + newPose.oZ == _poseQueue.last.oZ) { + return; + } } setState(() { _targetArmPose = newPose; @@ -239,12 +247,13 @@ class _ImuWidgetState extends State { // Add pose to queue _poseQueue.add(newPose); + debugPrint("new pose added to queue: ${newPose.x}, ${newPose.y}, ${newPose.z}"); _poseCounter++; // Start processing queue if not already processing - if (!_isProcessingQueue) { - _processQueueSequentially(); - } + // if (!_isProcessingQueue) { + // _processQueueSequentially(); + // } } catch (e) { setState(() { _lastError = e.toString(); @@ -257,6 +266,10 @@ class _ImuWidgetState extends State { /// Process pose queue sequentially, executing every 20th pose /// Instead of sending every pose immediately (which causes lag), we queue them /// and only execute every Nth pose, using the latest position from the queue + /// + /// + /// only add every 5th pose to the queue, seperate from the execution logic + /// exuection logic just runs first pose in the queue Future _processQueueSequentially() async { _isProcessingQueue = true; @@ -392,7 +405,14 @@ class _ImuWidgetState extends State { "Move your phone through space!", style: TextStyle(fontSize: 12, fontStyle: FontStyle.italic), ), - TextButton(onPressed: () async => await widget.arm.moveToPosition(_targetArmPose!), child: Text("Execute")) + TextButton( + onPressed: () async { + await widget.arm.moveToPosition(_targetArmPose!); + debugPrint("finished move to position"); + _setReference(); + }, + child: Text("Execute"), + ) ], ); } From 1307ec04a7a296e2279e374f2f567b3f6aed2be6 Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Thu, 6 Nov 2025 12:09:28 -0500 Subject: [PATCH 20/27] clean up --- .../lib/resources/arm_widgets/imu_widget.dart | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index aec70e34672..7c854133cd3 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -30,9 +30,6 @@ class _ImuWidgetState extends State { final _streamSubscriptions = >[]; Duration sensorInterval = SensorInterval.uiInterval; - DateTime? _lastArmCommandTime; - // Rate limit: 5 commands/sec - static const Duration _armCommandInterval = Duration(milliseconds: 200); static const double _positionScale = 300.0; static const double _velocityDecay = 0.99; @@ -170,17 +167,12 @@ class _ImuWidgetState extends State { final accelX = event.x.abs() > _deadZoneXY ? event.x : 0.0; final accelY = event.y.abs() > _deadZoneXY ? event.y : 0.0; final accelZ = event.z.abs() > _deadZoneZ ? event.z : 0.0; - print("event.x: ${event.x}, event.y: ${event.y}, event.z: ${event.z}"); - // final accelX = event.x; - // final accelY = event.y; - // final accelZ = event.z; if (accelX == 0.0 && accelY == 0.0 && accelZ == 0.0) { - // Phone is stationary, reset velocity to prevent drift from sensor noise _velocityX = 0.0; _velocityY = 0.0; _velocityZ = 0.0; - return; // Don't update position + return; } // Calculate velocity @@ -193,7 +185,7 @@ class _ImuWidgetState extends State { _velocityY *= _velocityDecay; _velocityZ *= _velocityDecay; - // Clamp very small velocities to zero to prevent creep + // Filter out very small velocities to prevent drift if (_velocityX.abs() < _velocityThreshold) _velocityX = 0.0; if (_velocityY.abs() < _velocityThreshold) _velocityY = 0.0; if (_velocityZ.abs() < _velocityThreshold) _velocityZ = 0.0; @@ -204,7 +196,6 @@ class _ImuWidgetState extends State { _positionZ += _velocityZ * dt; // Ready to create and queue the pose - _lastArmCommandTime = now; _isMovingArm = true; try { From 0730e96a95e835cb5a59f217c8bd1c496f3a49ed Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Thu, 6 Nov 2025 13:45:10 -0500 Subject: [PATCH 21/27] queue fixes --- .../lib/resources/arm_widgets/imu_widget.dart | 75 ++++++++----------- 1 file changed, 33 insertions(+), 42 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index 7c854133cd3..99345b60653 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -1,4 +1,5 @@ import 'dart:async'; +import 'dart:collection'; import 'dart:math' as math; import 'package:flutter/material.dart'; @@ -41,7 +42,7 @@ class _ImuWidgetState extends State { String? _lastError; // Pose queue for batching arm movements to reduce lag - final List _poseQueue = []; + final Queue _poseQueue = Queue(); int _poseCounter = 0; bool _isProcessingQueue = false; @@ -141,10 +142,6 @@ class _ImuWidgetState extends State { return; } - if (_isMovingArm) { - return; - } - final now = DateTime.now(); // Initialize integration time on first run @@ -172,7 +169,7 @@ class _ImuWidgetState extends State { _velocityX = 0.0; _velocityY = 0.0; _velocityZ = 0.0; - return; + return; } // Calculate velocity @@ -195,9 +192,6 @@ class _ImuWidgetState extends State { _positionY += _velocityY * dt; _positionZ += _velocityZ * dt; - // Ready to create and queue the pose - _isMovingArm = true; - try { // Calculate new target position based on reference + phone displacement // referenceArmPose is the arm's position in the real world when we set the reference @@ -237,61 +231,50 @@ class _ImuWidgetState extends State { }); // Add pose to queue - _poseQueue.add(newPose); - debugPrint("new pose added to queue: ${newPose.x}, ${newPose.y}, ${newPose.z}"); - _poseCounter++; - - // Start processing queue if not already processing - // if (!_isProcessingQueue) { - // _processQueueSequentially(); - // } + _addPoseToQueue(newPose, 5); + + if (!_isProcessingQueue) { + _executePoseFromQueue(); + } } catch (e) { setState(() { _lastError = e.toString(); }); - } finally { - _isMovingArm = false; } } - /// Process pose queue sequentially, executing every 20th pose - /// Instead of sending every pose immediately (which causes lag), we queue them - /// and only execute every Nth pose, using the latest position from the queue - /// - /// - /// only add every 5th pose to the queue, seperate from the execution logic - /// exuection logic just runs first pose in the queue - Future _processQueueSequentially() async { - _isProcessingQueue = true; + // Add every nth pose to the queue, + void _addPoseToQueue(Pose pose, int n) { + if (n == 0 || _poseCounter % n == 0) { + _poseQueue.addLast(pose); + debugPrint("new pose added to queue: ${pose.x}, ${pose.y}, ${pose.z}"); + } + _poseCounter++; + } + void _executePoseFromQueue() async { + _isProcessingQueue = true; while (_poseQueue.isNotEmpty) { - // Only execute every 20th pose - if (_poseCounter % 5 == 0) { - // Get the latest pose from the queue (skip intermediate ones) - final poseToExecute = _poseQueue.last; - _poseQueue.clear(); // Clear all accumulated poses - + if (!_isMovingArm) { + final poseToExecute = _poseQueue.first; + _poseQueue.removeFirst(); + _isMovingArm = true; try { await widget.arm.moveToPosition(poseToExecute); setState(() { - _lastError = null; _currentArmPose = poseToExecute; + _lastError = null; }); } catch (e) { setState(() { _lastError = e.toString(); }); } - } else { - // Skip this batch, just clear the queue - _poseQueue.clear(); + _isMovingArm = false; } - - // Small delay to allow new poses to accumulate - await Future.delayed(const Duration(milliseconds: 50)); } - _isProcessingQueue = false; + _poseCounter = 0; } /// Set reference point @@ -403,6 +386,14 @@ class _ImuWidgetState extends State { _setReference(); }, child: Text("Execute"), + ), + TextButton( + onPressed: () async { + setState(() { + _isReferenceSet = false; + }); + }, + child: Text("Stop"), ) ], ); From 5bb3b5dc96602275bf992ee391be1189ad896d79 Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Thu, 6 Nov 2025 17:01:10 -0500 Subject: [PATCH 22/27] comment out orientation since not working --- .../lib/resources/arm_widgets/imu_widget.dart | 223 +++++++++++------- 1 file changed, 140 insertions(+), 83 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index 99345b60653..1a596073240 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -31,11 +31,11 @@ class _ImuWidgetState extends State { final _streamSubscriptions = >[]; Duration sensorInterval = SensorInterval.uiInterval; - static const double _positionScale = 300.0; + static const double _positionScale = 600.0; static const double _velocityDecay = 0.99; - static const double _deadZoneZ = 0.3; + static const double _deadZoneZ = 0.5; static const double _deadZoneXY = 0.1; static const double _velocityThreshold = 0.01; // Threshold below which velocity is considered zero bool _isMovingArm = false; @@ -57,9 +57,9 @@ class _ImuWidgetState extends State { DateTime? _lastIntegrationTime; // Orientation (radians) - double _orientationX = 0.0; // Roll - double _orientationY = 0.0; // Pitch - double _orientationZ = 0.0; // Yaw + // double _orientationX = 0.0; // Roll + // double _orientationY = 0.0; // Pitch + // double _orientationZ = 0.0; // Yaw DateTime? _lastGyroIntegrationTime; Pose? _referenceArmPose; // arm position set once when you press "set reference" @@ -76,13 +76,14 @@ class _ImuWidgetState extends State { ), ); - _streamSubscriptions.add( - gyroscopeEventStream(samplingPeriod: sensorInterval).listen( - _updateOrientationFromGyroscope, - onError: (_) => _showSensorError("Gyroscope"), - cancelOnError: true, - ), - ); + /// Note: Orientation logic is commented out because we cannot convert to orientation vector without the spatial math package. + // _streamSubscriptions.add( + // gyroscopeEventStream(samplingPeriod: sensorInterval).listen( + // _updateOrientationFromGyroscope, + // onError: (_) => _showSensorError("Gyroscope"), + // cancelOnError: true, + // ), + // ); } void _showSensorError(String sensorName) { @@ -96,44 +97,42 @@ class _ImuWidgetState extends State { } /// Update orientation by integrating gyroscope angular velocity - void _updateOrientationFromGyroscope(GyroscopeEvent event) { - final now = DateTime.now(); - - // Initialize integration time on first run - if (_lastGyroIntegrationTime == null) { - _lastGyroIntegrationTime = now; - return; - } - - // Calculate time delta between now and last integration time (in seconds) so we know how long we've been rotating for. - // tldr: to get orientation, we need angular velocity * time. - final dt = now.difference(_lastGyroIntegrationTime!).inMilliseconds / 1000.0; - _lastGyroIntegrationTime = now; - - // Skip if dt is too large, meaning the phone has been stationary for too long in between movements. - if (dt > 0.5) { - return; - } - - // Don't update orientation if reference point hasn't been set - if (!_isReferenceSet) { - return; - } - - // Calucluate orientation change: integrate angular velocity over time to get orientation. - // Gyroscope values are in radians/second - // event.x = rotation rate around X-axis (roll) - // event.y = rotation rate around Y-axis (pitch) - // event.z = rotation rate around Z-axis (yaw) - _orientationX += event.x * dt; - _orientationY += event.y * dt; - _orientationZ += event.z * dt; - - // Apply small decay to prevent drift - _orientationX *= 0.999; - _orientationY *= 0.999; - _orientationZ *= 0.999; - } + // void _updateOrientationFromGyroscope(GyroscopeEvent event) { + // final now = DateTime.now(); + + // // Initialize integration time on first run + // if (_lastGyroIntegrationTime == null) { + // _lastGyroIntegrationTime = now; + // return; + // } + + // // Calculate time delta between now and last integration time (in seconds) so we know how long we've been rotating for. + // // tldr: to get orientation, we need angular velocity * time. + // final dt = now.difference(_lastGyroIntegrationTime!).inMilliseconds / 1000.0; + // _lastGyroIntegrationTime = now; + + // // Skip if dt is too large, meaning the phone has been stationary for too long in between movements. + // if (dt > 0.5) { + // return; + // } + + // // Don't update orientation if reference point hasn't been set + // if (!_isReferenceSet) { + // return; + // } + + // // Calucluate orientation change: integrate angular velocity over time to get orientation. + // // Gyroscope values are in radians/second + + // _orientationX += event.x * dt; // roll + // _orientationY += event.y * dt; // pitch + // _orientationZ += event.z * dt; // yaw + + // // Apply small decay to prevent drift + // _orientationX *= 0.999; + // _orientationY *= 0.999; + // _orientationZ *= 0.999; + // } /// Create a pose based on IMU accelerometer data using acceleration to get position /// The pose is added to a queue for sequential processing. @@ -195,25 +194,29 @@ class _ImuWidgetState extends State { try { // Calculate new target position based on reference + phone displacement // referenceArmPose is the arm's position in the real world when we set the reference - final newX = _referenceArmPose!.x + (_positionX * _positionScale); - final newY = _referenceArmPose!.y + (_positionY * _positionScale); + final newX = _referenceArmPose!.x + (_positionY * _positionScale); + final newY = _referenceArmPose!.y + ((-_positionX) * _positionScale); final newZ = _referenceArmPose!.z + (_positionZ * _positionScale); - final quaternion = vector_math.Quaternion.identity(); - quaternion.setEuler(_orientationZ, _orientationY, _orientationX); // Yaw, Pitch, Roll - final orientationVector = quatToOV(quaternion); - final newOrientationX = _referenceArmPose!.oX + orientationVector.x; - final newOrientationY = _referenceArmPose!.oY + orientationVector.y; - final newOrientationZ = _referenceArmPose!.oZ + orientationVector.z; + // Attempted to convert orientation values from angular velocities (yaw pitch roll) to orientation vectors + // Then convert orientation vectors to quaternions and multiply them to get the new arm orientation quaternion + // final phoneQuaternion = vector_math.Quaternion.identity(); + // phoneQuaternion.setEuler(_orientationZ, _orientationY, _orientationX); // Yaw, Pitch, Roll + // final armQuaternion = ovToQuat(_referenceArmPose!.oZ, _referenceArmPose!.oY, _referenceArmPose!.oX, _referenceArmPose!.theta); + // final newArmQuaternion = armQuaternion * phoneQuaternion; + // final newOrientationVector = quatToOV(newArmQuaternion); + // final newOrientationX = newOrientationVector.x; + // final newOrientationY = newOrientationVector.y; + // final newOrientationZ = newOrientationVector.z; final newPose = Pose( x: newX, y: newY, z: newZ, - theta: _referenceArmPose!.theta, // Keep theta from reference - oX: newOrientationX, - oY: newOrientationY, - oZ: newOrientationZ, + theta: _referenceArmPose!.theta, + oX: _referenceArmPose!.oX, // would be newOrientationX if math was correct + oY: _referenceArmPose!.oY, + oZ: _referenceArmPose!.oZ, ); if (_poseQueue.isNotEmpty) { @@ -273,6 +276,7 @@ class _ImuWidgetState extends State { _isMovingArm = false; } } + debugPrint("queue is empty"); _isProcessingQueue = false; _poseCounter = 0; } @@ -296,9 +300,9 @@ class _ImuWidgetState extends State { _lastIntegrationTime = null; // Zero out orientation tracking - _orientationX = 0.0; - _orientationY = 0.0; - _orientationZ = 0.0; + // _orientationX = 0.0; + // _orientationY = 0.0; + // _orientationZ = 0.0; _lastGyroIntegrationTime = null; // Clear pose queue and reset counter @@ -381,16 +385,11 @@ class _ImuWidgetState extends State { ), TextButton( onPressed: () async { - await widget.arm.moveToPosition(_targetArmPose!); - debugPrint("finished move to position"); - _setReference(); - }, - child: Text("Execute"), - ), - TextButton( - onPressed: () async { + await widget.arm.stop(); setState(() { _isReferenceSet = false; + _poseQueue.clear(); + _poseCounter = 0; }); }, child: Text("Stop"), @@ -399,9 +398,68 @@ class _ImuWidgetState extends State { ); } - /// Converts a unit quaternion (q) to an OrientationVector. + /// Converts an OrientationVector to a quaternion. /// - /// Converted from go code to flutter using gemin + /// Translated from Go spatialmath code - OrientationVector.Quaternion() + /// OX, OY, OZ represent a point on the unit sphere where the end effector is pointing + /// Theta is rotation around that pointing axis + vector_math.Quaternion ovToQuat(double oX, double oY, double oZ, double theta) { + const double defaultAngleEpsilon = 1e-4; + + // Normalize the orientation vector + final norm = math.sqrt(oX * oX + oY * oY + oZ * oZ); + double normOX = oX; + double normOY = oY; + double normOZ = oZ; + + if (norm == 0.0) { + // Default orientation: pointing up along Z axis + normOX = 0.0; + normOY = 0.0; + normOZ = -1.0; + } else { + normOX /= norm; + normOY /= norm; + normOZ /= norm; + } + + // acos(OZ) ranges from 0 (north pole) to pi (south pole) + final lat = math.acos(normOZ.clamp(-1.0, 1.0)); + + // If we're pointing at the Z axis then lon is 0, theta is the OV theta + double lon = 0.0; + + if (1 - normOZ.abs() > defaultAngleEpsilon) { + // If we are not at a pole, we need the longitude + lon = math.atan2(normOY, normOX); + } + + // Use ZYZ Euler angles to create quaternion + // This matches: mgl64.AnglesToQuat(lon, lat, theta, mgl64.ZYZ) + return _eulerZYZToQuat(lon, lat, theta); + } + + /// Convert ZYZ Euler angles to quaternion + /// Manually composing Q1(Z by z1) * Q2(Y by y) * Q3(Z by z2) + vector_math.Quaternion _eulerZYZToQuat(double z1, double y, double z2) { + // Create three rotation quaternions and compose them + // Q1: Rotation around Z axis by z1 + final q1 = vector_math.Quaternion.axisAngle(vector_math.Vector3(0, 0, 1), z1); + + // Q2: Rotation around Y axis by y + final q2 = vector_math.Quaternion.axisAngle(vector_math.Vector3(0, 1, 0), y); + + // Q3: Rotation around Z axis by z2 + final q3 = vector_math.Quaternion.axisAngle(vector_math.Vector3(0, 0, 1), z2); + + // Compose: Q = Q1 * Q2 * Q3 (intrinsic rotations) + final result = q1 * q2 * q3; + + return result; + } + + /// Converts a unit quaternion (q) to an OrientationVector. + /// Converted from go code to flutter using Gemini Orientation_OrientationVectorRadians quatToOV(vector_math.Quaternion q) { double orientationVectorPoleRadius = 0.0001; double defaultAngleEpsilon = 1e-4; @@ -413,7 +471,7 @@ class _ImuWidgetState extends State { final ov = Orientation_OrientationVectorRadians(); - // 1. Get the transform of our +X and +Z points (Quaternion rotation formula: q * v * q_conj) + // Get the transform of our +X and +Z points (Quaternion rotation formula: q * v * q_conj) final vector_math.Quaternion newX = q * xAxis * q.conjugated(); final vector_math.Quaternion newZ = q * zAxis * q.conjugated(); @@ -422,11 +480,11 @@ class _ImuWidgetState extends State { ov.y = newZ.y; ov.z = newZ.z; - // 2. Calculate the roll angle (Theta) + // Calculate the roll angle (Theta) // Check if we are near the poles (i.e., newZ.z/Kmag is close to 1 or -1) if (1 - (ov.z.abs()) > orientationVectorPoleRadius) { - // --- General Case: Not Near the Pole --- + // General Case: Not Near the Pole // Vector3 versions of the rotated axes final vector_math.Vector3 v1 = vector_math.Vector3(newZ.x, newZ.y, newZ.z); // Local Z @@ -441,7 +499,7 @@ class _ImuWidgetState extends State { // Find the angle (theta) between the two planes (using the angle between their normals) final double denominator = norm1.length * norm2.length; - final double cosTheta = denominator != 0.0 ? norm1.dot(norm2) / denominator : 1.0; // Avoid division by zero, default to 1 (0 angle) + final double cosTheta = norm1.dot(norm2) / denominator; // Avoid division by zero, default to 1 (0 angle) // Clamp for float error double clampedCosTheta = cosTheta.clamp(-1.0, 1.0); @@ -466,10 +524,10 @@ class _ImuWidgetState extends State { final double norm1Len = norm1.length; final double norm3Len = norm3.length; - final double cosTest = (norm1Len * norm3Len) != 0.0 ? norm1.dot(norm3) / (norm1Len * norm3Len) : 1.0; + final double cosTest = norm1.dot(norm3) / (norm1Len * norm3Len); // Check if norm1 and norm3 are coplanar (angle close to 0) - if (1.0 - cosTest.abs() < defaultAngleEpsilon * defaultAngleEpsilon) { + if (1.0 - cosTest < defaultAngleEpsilon * defaultAngleEpsilon) { ov.theta = -theta; } else { ov.theta = theta; @@ -478,7 +536,7 @@ class _ImuWidgetState extends State { ov.theta = 0.0; } } else { - // --- Special Case: Near the Pole (Z-axis is up or down) --- + // Special Case: Near the Pole (Z-axis is up or down) // Use Atan2 on the rotated X-axis components (Jmag and Imag, or y and x in Dart) // -math.Atan2(newX.Jmag, -newX.Imag) -> Dart: -math.atan2(newX.y, -newX.x) @@ -495,7 +553,6 @@ class _ImuWidgetState extends State { if (ov.theta == -0.0) { ov.theta = 0.0; } - return ov; } From ee514622d7091625a62702d8259dde0387de2aaf Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Thu, 6 Nov 2025 17:01:51 -0500 Subject: [PATCH 23/27] unused variable --- .../viam_example_app/lib/resources/arm_widgets/imu_widget.dart | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index 1a596073240..227911a9ed6 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -60,7 +60,7 @@ class _ImuWidgetState extends State { // double _orientationX = 0.0; // Roll // double _orientationY = 0.0; // Pitch // double _orientationZ = 0.0; // Yaw - DateTime? _lastGyroIntegrationTime; + // DateTime? _lastGyroIntegrationTime; Pose? _referenceArmPose; // arm position set once when you press "set reference" bool _isReferenceSet = false; From e09222490c89483f9659ae76e5dba2d941b42d1f Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Thu, 6 Nov 2025 17:06:30 -0500 Subject: [PATCH 24/27] another unused variable --- .../viam_example_app/lib/resources/arm_widgets/imu_widget.dart | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index 227911a9ed6..ba9cdac8115 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -303,7 +303,7 @@ class _ImuWidgetState extends State { // _orientationX = 0.0; // _orientationY = 0.0; // _orientationZ = 0.0; - _lastGyroIntegrationTime = null; + // _lastGyroIntegrationTime = null; // Clear pose queue and reset counter _poseQueue.clear(); From dda9aac1c179b186fbb14e6f486f9467dc2c996a Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Thu, 13 Nov 2025 13:40:25 -0500 Subject: [PATCH 25/27] changed x and y thresholds and added still button --- .../lib/resources/arm_widgets/imu_widget.dart | 30 ++++++++++++++----- 1 file changed, 22 insertions(+), 8 deletions(-) diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index ba9cdac8115..fdb2a2379eb 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -31,12 +31,14 @@ class _ImuWidgetState extends State { final _streamSubscriptions = >[]; Duration sensorInterval = SensorInterval.uiInterval; - static const double _positionScale = 600.0; + static const double _positionScale = 800.0; static const double _velocityDecay = 0.99; static const double _deadZoneZ = 0.5; - static const double _deadZoneXY = 0.1; + // static const double _deadZoneXY = 0.1; + static const double _deadZoneX = 0.18; + static const double _deadZoneY = 0.22; static const double _velocityThreshold = 0.01; // Threshold below which velocity is considered zero bool _isMovingArm = false; String? _lastError; @@ -66,6 +68,7 @@ class _ImuWidgetState extends State { bool _isReferenceSet = false; Pose? _targetArmPose; Pose? _currentArmPose; + bool stillPressed = false; void _initImu() { _streamSubscriptions.add( @@ -160,15 +163,19 @@ class _ImuWidgetState extends State { } // Apply dead zone to acceleration to filter out noise - final accelX = event.x.abs() > _deadZoneXY ? event.x : 0.0; - final accelY = event.y.abs() > _deadZoneXY ? event.y : 0.0; + final accelX = event.x.abs() > _deadZoneX ? event.x : 0.0; + final accelY = event.y.abs() > _deadZoneY ? event.y : 0.0; final accelZ = event.z.abs() > _deadZoneZ ? event.z : 0.0; + print("accelX: ${event.x}, accelY: ${event.y}, accelZ: ${event.z}"); - if (accelX == 0.0 && accelY == 0.0 && accelZ == 0.0) { + if (stillPressed) { + // if (accelX == 0.0 && accelY == 0.0 && accelZ == 0.0) { + print("still pressed and accel is 0"); _velocityX = 0.0; _velocityY = 0.0; _velocityZ = 0.0; return; + // } } // Calculate velocity @@ -234,7 +241,7 @@ class _ImuWidgetState extends State { }); // Add pose to queue - _addPoseToQueue(newPose, 5); + _addPoseToQueue(newPose, 0); if (!_isProcessingQueue) { _executePoseFromQueue(); @@ -250,7 +257,7 @@ class _ImuWidgetState extends State { void _addPoseToQueue(Pose pose, int n) { if (n == 0 || _poseCounter % n == 0) { _poseQueue.addLast(pose); - debugPrint("new pose added to queue: ${pose.x}, ${pose.y}, ${pose.z}"); + // debugPrint("new pose added to queue: ${pose.x}, ${pose.y}, ${pose.z}"); } _poseCounter++; } @@ -276,7 +283,7 @@ class _ImuWidgetState extends State { _isMovingArm = false; } } - debugPrint("queue is empty"); + // debugPrint("queue is empty"); _isProcessingQueue = false; _poseCounter = 0; } @@ -286,6 +293,7 @@ class _ImuWidgetState extends State { /// Clears position and orientation tracking so the phone starts at (0,0,0) relative to this reference. Future _setReference() async { try { + stillPressed = false; // Get the current arm position final currentArmPose = await widget.arm.endPosition(); @@ -383,6 +391,12 @@ class _ImuWidgetState extends State { "Move your phone through space!", style: TextStyle(fontSize: 12, fontStyle: FontStyle.italic), ), + TextButton( + onPressed: () async { + stillPressed = true; + }, + child: Text("Still"), + ), TextButton( onPressed: () async { await widget.arm.stop(); From 10387cacfa346b95018d7083dc596b333cdabbd3 Mon Sep 17 00:00:00 2001 From: martha-johnston Date: Mon, 17 Nov 2025 18:35:50 +0200 Subject: [PATCH 26/27] add spatial math library --- .../viam_example_app/spatialmath/common.dart | 177 ++++++++++++++++++ .../spatialmath/euler_angles.dart | 31 +++ .../spatialmath/orientation_vector.dart | 54 ++++++ .../spatialmath/quaternion.dart | 100 ++++++++++ .../spatialmath/spatial_math.dart | 15 ++ 5 files changed, 377 insertions(+) create mode 100644 example/viam_example_app/spatialmath/common.dart create mode 100644 example/viam_example_app/spatialmath/euler_angles.dart create mode 100644 example/viam_example_app/spatialmath/orientation_vector.dart create mode 100644 example/viam_example_app/spatialmath/quaternion.dart create mode 100644 example/viam_example_app/spatialmath/spatial_math.dart diff --git a/example/viam_example_app/spatialmath/common.dart b/example/viam_example_app/spatialmath/common.dart new file mode 100644 index 00000000000..9fa366edccb --- /dev/null +++ b/example/viam_example_app/spatialmath/common.dart @@ -0,0 +1,177 @@ +part of 'spatial_math.dart'; + +// --- Constants from Go Files --- + +/// How close OZ must be to +/-1 in order to use pole math for computing theta. +const double orientationVectorPoleRadius = 0.0001; + +/// A small epsilon value for float comparisons, assumed from context. +const double defaultAngleEpsilon = 1e-9; + +// --- Utility Helpers (Stubs for go.viam.com/rdk/utils) --- + +/// Stub for utils.RadToDeg +double radToDeg(double rad) { + return rad * (180.0 / math.pi); +} + +/// Stub for utils.DegToRad +double degToRad(double deg) { + return deg * (math.pi / 180.0); +} + +/// Stub for utils.Float64AlmostEqual +bool float64AlmostEqual(double a, double b, double tol) { + return (a - b).abs() <= tol; +} + +// --- Stubbed Dependent Classes (from mgl64, r3, etc.) --- + +/// Stub for r3.Vector +class R3Vector { + double x, y, z; + R3Vector(this.x, this.y, this.z); +} + +/// Stub for mgl64.Vec3 +class Vec3 { + double x, y, z; + Vec3(this.x, this.y, this.z); + + double dot(Vec3 other) { + return x * other.x + y * other.y + z * other.z; + } + + Vec3 cross(Vec3 other) { + return Vec3( + y * other.z - z * other.y, + z * other.x - x * other.z, + x * other.y - y * other.x, + ); + } + + double len() { + return math.sqrt(x * x + y * y + z * z); + } +} + +/// Stub for mgl64.Quat +class MGLQuat { + double w; + Vec3 v; + + MGLQuat(this.w, this.v); + + MGLQuat normalize() { + final l = math.sqrt(w * w + v.x * v.x + v.y * v.y + v.z * v.z); + if (l == 0) return MGLQuat(1, Vec3(0, 0, 0)); + return MGLQuat(w / l, Vec3(v.x / l, v.y / l, v.z / l)); + } + + MGLQuat scale(double s) { + return MGLQuat(w * s, Vec3(v.x * s, v.y * s, v.z * s)); + } + + double x() => v.x; + double y() => v.y; + double z() => v.z; + + /// Stub for mgl64.QuatSlerp + static MGLQuat slerp(MGLQuat q1, MGLQuat q2, double t) { + // A simplified slerp implementation + double cosHalfTheta = q1.w * q2.w + q1.v.dot(q2.v); + + if (cosHalfTheta.abs() >= 1.0) { + return q1; + } + + double halfTheta = math.acos(cosHalfTheta); + double sinHalfTheta = math.sqrt(1.0 - cosHalfTheta * cosHalfTheta); + + if (sinHalfTheta.abs() < 0.001) { + return MGLQuat( + q1.w * 0.5 + q2.w * 0.5, + Vec3( + q1.v.x * 0.5 + q2.v.x * 0.5, + q1.v.y * 0.5 + q2.v.y * 0.5, + q1.v.z * 0.5 + q2.v.z * 0.5, + ), + ); + } + + double ratioA = math.sin((1 - t) * halfTheta) / sinHalfTheta; + double ratioB = math.sin(t * halfTheta) / sinHalfTheta; + + return MGLQuat( + (q1.w * ratioA + q2.w * ratioB), + Vec3( + (q1.v.x * ratioA + q2.v.x * ratioB), + (q1.v.y * ratioA + q2.v.y * ratioB), + (q1.v.z * ratioA + q2.v.z * ratioB), + ), + ); + } + + /// Stub for mgl64.QuatNlerp + static MGLQuat nlerp(MGLQuat q1, MGLQuat q2, double t) { + // Simplified nlerp + final q = MGLQuat( + (1 - t) * q1.w + t * q2.w, + Vec3( + (1 - t) * q1.v.x + t * q2.v.x, + (1 - t) * q1.v.y + t * q2.v.y, + (1 - t) * q1.v.z + t * q2.v.z, + ), + ); + return q.normalize(); + } + + /// Stub for mgl64.AnglesToQuat(lon, lat, theta, mgl64.ZYZ) + static MGLQuat anglesToQuat(double z1, double y, double z2) { + // ZYZ Euler to Quaternion + final c1 = math.cos(z1 / 2); + final s1 = math.sin(z1 / 2); + final c2 = math.cos(y / 2); + final s2 = math.sin(y / 2); + final c3 = math.cos(z2 / 2); + final s3 = math.sin(z2 / 2); + + return MGLQuat( + c1 * c2 * c3 - s1 * c2 * s3, // w + Vec3( + c1 * s2 * c3 - s1 * s2 * s3, // x + c1 * s2 * s3 + s1 * s2 * c3, // y + s1 * c2 * c3 + c1 * c2 * s3, // z + ), + ); + } +} + +/// Stub for R4AA (Axis-Angle) +class R4AA { + double theta, rx, ry, rz; + R4AA(this.theta, this.rx, this.ry, this.rz); + + /// Stub for R4AA.ToQuat() + Quaternion toQuat() { + final halfAngle = theta / 2.0; + final s = math.sin(halfAngle); + return Quaternion( + math.cos(halfAngle), + rx * s, + ry * s, + rz * s, + ); + } +} + +/// Stub for RotationMatrix +class RotationMatrix { + List mat; // Expects a list of 9 doubles + RotationMatrix(this.mat); +} + +/// Stub for NewZeroOrientation() +Quaternion newZeroOrientation() { + return Quaternion(1, 0, 0, 0); // Identity quaternion +} diff --git a/example/viam_example_app/spatialmath/euler_angles.dart b/example/viam_example_app/spatialmath/euler_angles.dart new file mode 100644 index 00000000000..5f03a10f6a9 --- /dev/null +++ b/example/viam_example_app/spatialmath/euler_angles.dart @@ -0,0 +1,31 @@ +part of 'spatial_math.dart'; + +class EulerAngles { + double roll; + double pitch; + double yaw; + + EulerAngles(this.roll, this.pitch, this.yaw); + + EulerAngles.zero() + : roll = 0.0, + pitch = 0.0, + yaw = 0.0; + + Quaternion toQuaternion() { + final cy = math.cos(yaw * 0.5); + final sy = math.sin(yaw * 0.5); + final cp = math.cos(pitch * 0.5); + final sp = math.sin(pitch * 0.5); + final cr = math.cos(roll * 0.5); + final sr = math.sin(roll * 0.5); + + final q = Quaternion.zero(); + q.real = cr * cp * cy + sr * sp * sy; + q.imag = sr * cp * cy - cr * sp * sy; + q.jmag = cr * sp * cy + sr * cp * sy; + q.kmag = cr * cp * sy - sr * sp * cy; + + return q; + } +} diff --git a/example/viam_example_app/spatialmath/orientation_vector.dart b/example/viam_example_app/spatialmath/orientation_vector.dart new file mode 100644 index 00000000000..fbd37a6b55a --- /dev/null +++ b/example/viam_example_app/spatialmath/orientation_vector.dart @@ -0,0 +1,54 @@ +part of 'spatial_math.dart'; + +class OrientationVector { + double theta; + double ox; + double oy; + double oz; + + OrientationVector(this.theta, this.ox, this.oy, this.oz); + + OrientationVector.zero() + : theta = 0.0, + ox = 0.0, + oy = 0.0, + oz = 1.0; + + double _computeNormal() { + return math.sqrt(ox * ox + oy * oy + oz * oz); + } + + String? isValid() { + if (_computeNormal() == 0.0) { + return "OrientationVector has a normal of 0, probably X, Y, and Z are all 0"; + } + return null; + } + + void normalize() { + final norm = _computeNormal(); + if (norm == 0.0) { + oz = 1; + return; + } + ox /= norm; + oy /= norm; + oz /= norm; + } + + Quaternion toQuaternion() { + normalize(); + + final lat = math.acos(oz); + + double lon = 0.0; + final th = theta; + + if (1 - oz.abs() > defaultAngleEpsilon) { + lon = math.atan2(oy, ox); + } + + final q1 = MGLQuat.anglesToQuat(lon, lat, th); // not confident this is doing a ZYZ rotation correctly + return Quaternion(q1.w, q1.x(), q1.y(), q1.z()); + } +} diff --git a/example/viam_example_app/spatialmath/quaternion.dart b/example/viam_example_app/spatialmath/quaternion.dart new file mode 100644 index 00000000000..2ef8ef4d49c --- /dev/null +++ b/example/viam_example_app/spatialmath/quaternion.dart @@ -0,0 +1,100 @@ +part of 'spatial_math.dart'; + +class Quaternion { + double real; // W + double imag; // X + double jmag; // Y + double kmag; // Z + + Quaternion(this.real, this.imag, this.jmag, this.kmag); + + Quaternion.identity() + : real = 1.0, + imag = 0.0, + jmag = 0.0, + kmag = 0.0; + + Quaternion.zero() + : real = 0.0, + imag = 0.0, + jmag = 0.0, + kmag = 0.0; + + OrientationVector toOrientationVectorRadians() { + return quatToOV(this); + } + + Quaternion conj() { + return Quaternion(real, -imag, -jmag, -kmag); + } + + // add quaternions by using this multiplication function + Quaternion mul(Quaternion other) { + return Quaternion( + real * other.real - imag * other.imag - jmag * other.jmag - kmag * other.kmag, + real * other.imag + imag * other.real + jmag * other.kmag - kmag * other.jmag, + real * other.jmag - imag * other.kmag + jmag * other.real + kmag * other.imag, + real * other.kmag + imag * other.jmag - jmag * other.imag + kmag * other.real, + ); + } +} + +OrientationVector quatToOV(Quaternion q) { + final xAxis = Quaternion(0, -1, 0, 0); + final zAxis = Quaternion(0, 0, 0, 1); + final ov = OrientationVector.zero(); + // Get the transform of our +X and +Z points + final newX = q.mul(xAxis).mul(q.conj()); + final newZ = q.mul(zAxis).mul(q.conj()); + ov.ox = newZ.imag; + ov.oy = newZ.jmag; + ov.oz = newZ.kmag; + + if (1 - newZ.kmag.abs() > orientationVectorPoleRadius) { + final v1 = Vec3(newZ.imag, newZ.jmag, + newZ.kmag); // might have to do something weird here to get a vector that doesn't distinguish between row and column + final v2 = Vec3(newX.imag, newX.jmag, newX.kmag); + + final norm1 = v1.cross(v2); + final norm2 = v1.cross(Vec3(zAxis.imag, zAxis.jmag, zAxis.kmag)); + + double cosTheta = norm1.dot(norm2) / (norm1.len() * norm2.len()); + if (cosTheta > 1) cosTheta = 1; + if (cosTheta < -1) cosTheta = -1; + + final theta = math.acos(cosTheta); + if (theta > orientationVectorPoleRadius) { + final aa = R4AA(-theta, ov.ox, ov.oy, ov.oz); + final q2 = aa.toQuat(); + final testZ = q2.mul(zAxis).mul(q2.conj()); + final norm3 = v1.cross(Vec3(testZ.imag, testZ.jmag, testZ.kmag)); + final cosTest = norm1.dot(norm3) / (norm1.len() * norm3.len()); + if (1 - cosTest < defaultAngleEpsilon * defaultAngleEpsilon) { + ov.theta = -theta; + } else { + ov.theta = theta; + } + } else { + ov.theta = 0; + } + } else { + // Special case for when we point directly along the Z axis + ov.theta = -math.atan2(newX.jmag, -newX.imag); + if (newZ.kmag < 0) { + ov.theta = -math.atan2(newX.jmag, newX.imag); + } + } + + if (ov.theta == -0.0) { + ov.theta = 0.0; + } + + return ov; +} + +bool quaternionAlmostEqual(Quaternion a, Quaternion b, double tol) { + return float64AlmostEqual(a.imag, b.imag, tol) && + float64AlmostEqual(a.jmag, b.jmag, tol) && + float64AlmostEqual(a.kmag, b.kmag, tol) && + float64AlmostEqual(a.real, b.real, tol); +} diff --git a/example/viam_example_app/spatialmath/spatial_math.dart b/example/viam_example_app/spatialmath/spatial_math.dart new file mode 100644 index 00000000000..58c94cc3321 --- /dev/null +++ b/example/viam_example_app/spatialmath/spatial_math.dart @@ -0,0 +1,15 @@ +library spatial_math; + +import 'dart:math' as math; + +// These 'part' files will be combined into this single library. +// This allows them to access each other's classes and functions +// without circular import errors. +part 'common.dart'; +part 'euler_angles.dart'; +part 'orientation_vector.dart'; +part 'quaternion.dart'; + +// All public classes in the 'part' files are automatically +// exported as part of the 'spatial_math' library. +// The previous 'export' lines were incorrect and have been removed. From 15c36eb95aaef105e153e8af12f1abd83f5de75c Mon Sep 17 00:00:00 2001 From: Julie Krasnick Date: Mon, 17 Nov 2025 17:40:44 -0500 Subject: [PATCH 27/27] adding arkit and also moving spatial math package --- .../ios/Flutter/AppFrameworkInfo.plist | 2 +- example/viam_example_app/ios/Podfile | 2 +- .../ios/Runner.xcodeproj/project.pbxproj | 6 +- .../xcshareddata/xcschemes/Runner.xcscheme | 3 + .../xcshareddata/IDEWorkspaceChecks.plist | 8 - .../xcshareddata/WorkspaceSettings.xcsettings | 8 - .../viam_example_app/ios/Runner/Info.plist | 4 + .../lib/resources/arm_screen.dart | 4 +- .../resources/arm_widgets/arkit_widget.dart | 409 ++++++++++++++++ .../lib/resources/arm_widgets/imu_widget.dart | 446 +++++++++--------- .../{ => lib}/spatialmath/common.dart | 0 .../{ => lib}/spatialmath/euler_angles.dart | 0 .../spatialmath/orientation_vector.dart | 0 .../{ => lib}/spatialmath/quaternion.dart | 0 .../{ => lib}/spatialmath/spatial_math.dart | 0 example/viam_example_app/pubspec.yaml | 1 + 16 files changed, 661 insertions(+), 232 deletions(-) delete mode 100644 example/viam_example_app/ios/Runner.xcworkspace/xcshareddata/IDEWorkspaceChecks.plist delete mode 100644 example/viam_example_app/ios/Runner.xcworkspace/xcshareddata/WorkspaceSettings.xcsettings create mode 100644 example/viam_example_app/lib/resources/arm_widgets/arkit_widget.dart rename example/viam_example_app/{ => lib}/spatialmath/common.dart (100%) rename example/viam_example_app/{ => lib}/spatialmath/euler_angles.dart (100%) rename example/viam_example_app/{ => lib}/spatialmath/orientation_vector.dart (100%) rename example/viam_example_app/{ => lib}/spatialmath/quaternion.dart (100%) rename example/viam_example_app/{ => lib}/spatialmath/spatial_math.dart (100%) diff --git a/example/viam_example_app/ios/Flutter/AppFrameworkInfo.plist b/example/viam_example_app/ios/Flutter/AppFrameworkInfo.plist index 7c569640062..1dc6cf7652b 100644 --- a/example/viam_example_app/ios/Flutter/AppFrameworkInfo.plist +++ b/example/viam_example_app/ios/Flutter/AppFrameworkInfo.plist @@ -21,6 +21,6 @@ CFBundleVersion 1.0 MinimumOSVersion - 12.0 + 13.0 diff --git a/example/viam_example_app/ios/Podfile b/example/viam_example_app/ios/Podfile index 79d16c83ed2..53732e912f3 100644 --- a/example/viam_example_app/ios/Podfile +++ b/example/viam_example_app/ios/Podfile @@ -45,7 +45,7 @@ post_install do |installer| installer.generated_projects.each do |project| project.targets.each do |target| target.build_configurations.each do |config| - config.build_settings['IPHONEOS_DEPLOYMENT_TARGET'] = '12.0' + config.build_settings['IPHONEOS_DEPLOYMENT_TARGET'] = '13.0' end end end diff --git a/example/viam_example_app/ios/Runner.xcodeproj/project.pbxproj b/example/viam_example_app/ios/Runner.xcodeproj/project.pbxproj index ded87b019f0..67cfee2bedb 100644 --- a/example/viam_example_app/ios/Runner.xcodeproj/project.pbxproj +++ b/example/viam_example_app/ios/Runner.xcodeproj/project.pbxproj @@ -453,7 +453,7 @@ GCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE; GCC_WARN_UNUSED_FUNCTION = YES; GCC_WARN_UNUSED_VARIABLE = YES; - IPHONEOS_DEPLOYMENT_TARGET = 12.0; + IPHONEOS_DEPLOYMENT_TARGET = 13.0; MTL_ENABLE_DEBUG_INFO = NO; SDKROOT = iphoneos; SUPPORTED_PLATFORMS = iphoneos; @@ -580,7 +580,7 @@ GCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE; GCC_WARN_UNUSED_FUNCTION = YES; GCC_WARN_UNUSED_VARIABLE = YES; - IPHONEOS_DEPLOYMENT_TARGET = 12.0; + IPHONEOS_DEPLOYMENT_TARGET = 13.0; MTL_ENABLE_DEBUG_INFO = YES; ONLY_ACTIVE_ARCH = YES; SDKROOT = iphoneos; @@ -629,7 +629,7 @@ GCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE; GCC_WARN_UNUSED_FUNCTION = YES; GCC_WARN_UNUSED_VARIABLE = YES; - IPHONEOS_DEPLOYMENT_TARGET = 12.0; + IPHONEOS_DEPLOYMENT_TARGET = 13.0; MTL_ENABLE_DEBUG_INFO = NO; SDKROOT = iphoneos; SUPPORTED_PLATFORMS = iphoneos; diff --git a/example/viam_example_app/ios/Runner.xcodeproj/xcshareddata/xcschemes/Runner.xcscheme b/example/viam_example_app/ios/Runner.xcodeproj/xcshareddata/xcschemes/Runner.xcscheme index 8e3ca5dfe19..e3773d42e24 100644 --- a/example/viam_example_app/ios/Runner.xcodeproj/xcshareddata/xcschemes/Runner.xcscheme +++ b/example/viam_example_app/ios/Runner.xcodeproj/xcshareddata/xcschemes/Runner.xcscheme @@ -26,6 +26,7 @@ buildConfiguration = "Debug" selectedDebuggerIdentifier = "Xcode.DebuggerFoundation.Debugger.LLDB" selectedLauncherIdentifier = "Xcode.DebuggerFoundation.Launcher.LLDB" + customLLDBInitFile = "$(SRCROOT)/Flutter/ephemeral/flutter_lldbinit" shouldUseLaunchSchemeArgsEnv = "YES"> diff --git a/example/viam_example_app/ios/Runner.xcworkspace/xcshareddata/IDEWorkspaceChecks.plist b/example/viam_example_app/ios/Runner.xcworkspace/xcshareddata/IDEWorkspaceChecks.plist deleted file mode 100644 index 18d981003d6..00000000000 --- a/example/viam_example_app/ios/Runner.xcworkspace/xcshareddata/IDEWorkspaceChecks.plist +++ /dev/null @@ -1,8 +0,0 @@ - - - - - IDEDidComputeMac32BitWarning - - - diff --git a/example/viam_example_app/ios/Runner.xcworkspace/xcshareddata/WorkspaceSettings.xcsettings b/example/viam_example_app/ios/Runner.xcworkspace/xcshareddata/WorkspaceSettings.xcsettings deleted file mode 100644 index f9b0d7c5ea1..00000000000 --- a/example/viam_example_app/ios/Runner.xcworkspace/xcshareddata/WorkspaceSettings.xcsettings +++ /dev/null @@ -1,8 +0,0 @@ - - - - - PreviewsEnabled - - - diff --git a/example/viam_example_app/ios/Runner/Info.plist b/example/viam_example_app/ios/Runner/Info.plist index 7c7c78ebdbb..3e3c2419a8f 100644 --- a/example/viam_example_app/ios/Runner/Info.plist +++ b/example/viam_example_app/ios/Runner/Info.plist @@ -10,6 +10,10 @@ NSMotionUsageDescription This app requires access to the barometer to provide altitude information. + NSCameraUsageDescription + This app requires camera access for AR functionality. + NSARKitUsageDescription + This app uses ARKit to track your phone's position in space. CADisableMinimumFrameDurationOnPhone CFBundleDevelopmentRegion diff --git a/example/viam_example_app/lib/resources/arm_screen.dart b/example/viam_example_app/lib/resources/arm_screen.dart index e064d357d19..65f15a25d3b 100644 --- a/example/viam_example_app/lib/resources/arm_screen.dart +++ b/example/viam_example_app/lib/resources/arm_screen.dart @@ -1,4 +1,5 @@ import 'package:flutter/material.dart'; +import 'package:viam_example_app/resources/arm_widgets/arkit_widget.dart'; import 'package:viam_example_app/resources/arm_widgets/imu_widget.dart'; import 'package:viam_sdk/viam_sdk.dart'; @@ -16,7 +17,8 @@ class ViamArmWidgetNew extends StatelessWidget { Widget build(BuildContext context) { return Column( children: [ - ImuWidget(arm: arm, updateNotifier: ArmNotifier()), + // ImuWidget(arm: arm, updateNotifier: ArmNotifier()), + ARKitArmWidget(arm: arm, updateNotifier: ArmNotifier()), ], ); } diff --git a/example/viam_example_app/lib/resources/arm_widgets/arkit_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/arkit_widget.dart new file mode 100644 index 00000000000..8808c396916 --- /dev/null +++ b/example/viam_example_app/lib/resources/arm_widgets/arkit_widget.dart @@ -0,0 +1,409 @@ +import 'dart:async'; +import 'dart:collection'; + +import 'package:arkit_plugin/arkit_plugin.dart'; +import 'package:flutter/material.dart'; +import 'package:vector_math/vector_math_64.dart' as vector_math; +import 'package:viam_example_app/resources/arm_screen.dart'; +import 'package:viam_sdk/viam_sdk.dart'; + +class ARKitArmWidget extends StatefulWidget { + final Arm arm; + final ArmNotifier updateNotifier; + + const ARKitArmWidget({ + super.key, + required this.arm, + required this.updateNotifier, + }); + + @override + State createState() => _ARKitArmWidgetState(); +} + +class _ARKitArmWidgetState extends State { + ARKitController? arkitController; + + static const double _positionScale = 800.0; + + bool _isMovingArm = false; + bool _isARKitInitialized = false; + String? _lastError; + + // Pose queue for batching arm movements to reduce lag + final Queue _poseQueue = Queue(); + int _poseCounter = 0; + bool _isProcessingQueue = false; + + // Phone position from ARKit (in meters) + vector_math.Vector3 _phonePosition = vector_math.Vector3.zero(); + + // Reference positions + Pose? _referenceArmPose; // arm position when reference is set + vector_math.Vector3? _referencePhonePosition; // phone position when reference is set + bool _isReferenceSet = false; + + Pose? _targetArmPose; + Pose? _currentArmPose; + bool _stillPressed = false; + + @override + void dispose() { + try { + if (arkitController != null) { + arkitController!.updateAtTime = null; + arkitController!.dispose(); + } + } catch (e) { + print('Error disposing ARKit controller: $e'); + } + _poseQueue.clear(); + super.dispose(); + } + + void onARKitViewCreated(ARKitController controller) { + try { + arkitController = controller; + + setState(() { + _isARKitInitialized = true; + _lastError = null; + }); + + // Called every frame (~60fps) - get camera transform and update position + arkitController!.updateAtTime = (time) { + if (!_isReferenceSet || _stillPressed || arkitController == null) return; + + // Use non-blocking approach - get transform without await + arkitController!.pointOfViewTransform().then((transform) { + if (transform != null && mounted) { + try { + // Extract position from transform matrix (4th column) + // negate X + _phonePosition = vector_math.Vector3( + transform[12], + transform[13], + transform[14], + ); + + // Create pose and send to arm + _createPoseFromARKit(); + } catch (e) { + if (mounted) { + setState(() { + _lastError = 'Error processing camera transform: $e'; + }); + } + } + } + }).catchError((e) { + if (mounted) { + setState(() { + _lastError = 'Error getting camera transform: $e'; + }); + } + }); + }; + } catch (e) { + setState(() { + _lastError = 'Failed to initialize ARKit: $e'; + _isARKitInitialized = false; + }); + } + } + + /// Create a pose based on ARKit camera position + void _createPoseFromARKit() { + if (_referenceArmPose == null || _referencePhonePosition == null) { + return; + } + try { + // Calculate position delta from reference + final positionDelta = _phonePosition - _referencePhonePosition!; + + // Calculate new arm position based on reference + phone displacement + // Convert meters to millimeters and scale appropriately + // ARKit coordinate system: X (right), Y (up), Z (towards user) + // Mapping: ARKit X -> Arm Y, ARKit Y -> Arm Z, ARKit Z -> -Arm X + final newX = _referenceArmPose!.x + (-positionDelta.z * _positionScale); + final newY = _referenceArmPose!.y + ((-positionDelta.x) * _positionScale); + final newZ = _referenceArmPose!.z + (positionDelta.y * _positionScale); + + final newPose = Pose( + x: newX, + y: newY, + z: newZ, + // Keep reference orientation unchanged for now + theta: _referenceArmPose!.theta, + oX: _referenceArmPose!.oX, + oY: _referenceArmPose!.oY, + oZ: _referenceArmPose!.oZ, + ); + + // Skip if pose hasn't changed significantly + if (_poseQueue.isNotEmpty) { + final lastPose = _poseQueue.last; + if ((newPose.x - lastPose.x).abs() < 1.0 && (newPose.y - lastPose.y).abs() < 1.0 && (newPose.z - lastPose.z).abs() < 1.0) { + return; + } + } + + setState(() { + _targetArmPose = newPose; + }); + + // Add pose to queue (every 5th pose to reduce load) + _addPoseToQueue(newPose, 0); + + if (!_isProcessingQueue) { + _executePoseFromQueue(); + } + } catch (e) { + setState(() { + _lastError = e.toString(); + }); + } + } + + void _addPoseToQueue(Pose pose, int n) { + if (n == 0 || _poseCounter % n == 0) { + _poseQueue.addLast(pose); + } + _poseCounter++; + } + + void _executePoseFromQueue() async { + _isProcessingQueue = true; + while (_poseQueue.isNotEmpty) { + if (!_isMovingArm) { + final poseToExecute = _poseQueue.first; + _poseQueue.removeFirst(); + _isMovingArm = true; + try { + await widget.arm.moveToPosition(poseToExecute); + setState(() { + _currentArmPose = poseToExecute; + _lastError = null; + }); + } catch (e) { + setState(() { + _lastError = e.toString(); + }); + } + _isMovingArm = false; + } + } + _isProcessingQueue = false; + _poseCounter = 0; + } + + /// Set reference point + Future _setReference() async { + if (arkitController == null || !_isARKitInitialized) { + setState(() { + _lastError = 'ARKit is not initialized yet'; + }); + return; + } + + try { + _stillPressed = false; + + // Get the current arm position + final currentArmPose = await widget.arm.endPosition(); + + // Get current ARKit camera position + final transform = await arkitController!.pointOfViewTransform(); + debugPrint("transformX: ${transform?[12]}, transformY: ${transform?[13]}, transformZ: ${transform?[14]}"); + + if (transform == null) { + setState(() { + _lastError = 'Failed to get ARKit camera transform'; + }); + return; + } + + // Extract position (4th column of transform matrix) + // Apply same transformation as IMU: flip X and Y, negate X + final currentPhonePosition = vector_math.Vector3( + // transform[13], // x uses Y + transform[12], // y uses negative X + // transform[12], + transform[13], + transform[14], // Z + ); + + setState(() { + // Clear pose queue and reset counter + _poseQueue.clear(); + _poseCounter = 0; + + // Store references + _referenceArmPose = currentArmPose; + _referencePhonePosition = currentPhonePosition; + + _targetArmPose = currentArmPose; + _currentArmPose = currentArmPose; + _isReferenceSet = true; + _lastError = null; + }); + } catch (e) { + setState(() { + _lastError = "Failed to set reference: ${e.toString()}"; + }); + } + } + + @override + Widget build(BuildContext context) { + return SizedBox( + height: MediaQuery.of(context).size.height, + child: Column( + children: [ + // AR View + Expanded( + flex: 2, + child: Stack( + children: [ + ARKitSceneView( + onARKitViewCreated: onARKitViewCreated, + enableTapRecognizer: false, + showStatistics: false, + ), + if (!_isARKitInitialized) + Container( + color: Colors.black.withOpacity(0.8), + child: const Center( + child: Column( + mainAxisAlignment: MainAxisAlignment.center, + children: [ + CircularProgressIndicator(color: Colors.white), + SizedBox(height: 20), + Text( + 'Initializing ARKit...', + style: TextStyle(color: Colors.white, fontSize: 16), + ), + ], + ), + ), + ), + if (_isReferenceSet) + Positioned( + top: 10, + left: 10, + child: Container( + padding: const EdgeInsets.all(8), + decoration: BoxDecoration( + color: Colors.black.withOpacity(0.7), + borderRadius: BorderRadius.circular(8), + ), + child: Column( + crossAxisAlignment: CrossAxisAlignment.start, + children: [ + const Text('Phone Position (m)', + style: TextStyle(color: Colors.white, fontSize: 10, fontWeight: FontWeight.bold)), + Text('X: ${_phonePosition.x.toStringAsFixed(3)}', style: const TextStyle(color: Colors.white, fontSize: 10)), + Text('Y: ${_phonePosition.y.toStringAsFixed(3)}', style: const TextStyle(color: Colors.white, fontSize: 10)), + Text('Z: ${_phonePosition.z.toStringAsFixed(3)}', style: const TextStyle(color: Colors.white, fontSize: 10)), + ], + ), + ), + ), + ], + ), + ), + + // Control Panel + Expanded( + flex: 1, + child: SingleChildScrollView( + padding: const EdgeInsets.all(16), + child: Column( + children: [ + const Text("Target Position", style: TextStyle(fontWeight: FontWeight.bold, fontSize: 18, color: Colors.blue)), + const SizedBox(height: 10), + if (_targetArmPose != null) ...[ + Text("X: ${_targetArmPose!.x.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + Text("Y: ${_targetArmPose!.y.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + Text("Z: ${_targetArmPose!.z.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + ] else + const Text("No target yet", style: TextStyle(fontSize: 12, color: Colors.grey)), + const SizedBox(height: 20), + const Text("Current Position", style: TextStyle(fontWeight: FontWeight.bold, fontSize: 18, color: Colors.green)), + const SizedBox(height: 10), + if (_currentArmPose != null) ...[ + Text("X: ${_currentArmPose!.x.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + Text("Y: ${_currentArmPose!.y.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + Text("Z: ${_currentArmPose!.z.toStringAsFixed(1)} mm", style: const TextStyle(fontSize: 14)), + ] else + const Text("No position data yet", style: TextStyle(fontSize: 12, color: Colors.grey)), + const SizedBox(height: 30), + ElevatedButton( + onPressed: _isARKitInitialized ? _setReference : null, + style: ElevatedButton.styleFrom( + backgroundColor: _isReferenceSet ? Colors.green : Colors.blue, + ), + child: Text(_isReferenceSet ? "Reset Reference" : "Set Reference Point"), + ), + const SizedBox(height: 15), + Text("Status: ${!_isReferenceSet ? 'Waiting for reference...' : _isMovingArm ? 'Moving...' : 'Ready'}"), + if (_lastError != null) + Padding( + padding: const EdgeInsets.all(8.0), + child: Text( + "Error: $_lastError", + style: const TextStyle(color: Colors.red, fontSize: 10), + textAlign: TextAlign.center, + ), + ), + const SizedBox(height: 15), + if (!_isReferenceSet) + const Text( + "Press 'Set Reference Point' to begin", + style: TextStyle(fontSize: 12, fontStyle: FontStyle.italic, color: Colors.orange), + ) + else + const Text( + "Move your phone through space!", + style: TextStyle(fontSize: 12, fontStyle: FontStyle.italic), + ), + Row( + mainAxisAlignment: MainAxisAlignment.spaceEvenly, + children: [ + TextButton( + onPressed: () { + setState(() { + _stillPressed = true; + }); + }, + child: const Text("Still"), + ), + TextButton( + onPressed: () async { + await widget.arm.stop(); + setState(() { + _isReferenceSet = false; + _poseQueue.clear(); + _poseCounter = 0; + }); + }, + child: const Text("Stop"), + ), + ], + ), + ElevatedButton( + onPressed: () async { + await widget.arm.moveToJointPositions([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]); + await widget.arm.moveToPosition(Pose(x: 300, y: 0, z: 100, oX: 0, oY: 0, oZ: -1, theta: 0)); + }, + child: const Text("Reset Position"), + ), + ], + ), + ), + ), + ], + ), + ); + } +} diff --git a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart index fdb2a2379eb..736f988a4c2 100644 --- a/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart +++ b/example/viam_example_app/lib/resources/arm_widgets/imu_widget.dart @@ -1,14 +1,13 @@ import 'dart:async'; import 'dart:collection'; -import 'dart:math' as math; import 'package:flutter/material.dart'; import 'package:sensors_plus/sensors_plus.dart'; -import 'package:vector_math/vector_math.dart' as vector_math; import 'package:viam_example_app/resources/arm_screen.dart'; -import 'package:viam_sdk/protos/app/robot.dart'; import 'package:viam_sdk/viam_sdk.dart'; +import '../../spatialmath/spatial_math.dart'; + class ImuWidget extends StatefulWidget { final Arm arm; final ArmNotifier updateNotifier; @@ -37,8 +36,8 @@ class _ImuWidgetState extends State { static const double _deadZoneZ = 0.5; // static const double _deadZoneXY = 0.1; - static const double _deadZoneX = 0.18; - static const double _deadZoneY = 0.22; + static const double _deadZoneX = 0.18; + static const double _deadZoneY = 0.22; static const double _velocityThreshold = 0.01; // Threshold below which velocity is considered zero bool _isMovingArm = false; String? _lastError; @@ -59,10 +58,10 @@ class _ImuWidgetState extends State { DateTime? _lastIntegrationTime; // Orientation (radians) - // double _orientationX = 0.0; // Roll - // double _orientationY = 0.0; // Pitch - // double _orientationZ = 0.0; // Yaw - // DateTime? _lastGyroIntegrationTime; + double _orientationX = 0.0; // Roll + double _orientationY = 0.0; // Pitch + double _orientationZ = 0.0; // Yaw + DateTime? _lastGyroIntegrationTime; Pose? _referenceArmPose; // arm position set once when you press "set reference" bool _isReferenceSet = false; @@ -80,13 +79,13 @@ class _ImuWidgetState extends State { ); /// Note: Orientation logic is commented out because we cannot convert to orientation vector without the spatial math package. - // _streamSubscriptions.add( - // gyroscopeEventStream(samplingPeriod: sensorInterval).listen( - // _updateOrientationFromGyroscope, - // onError: (_) => _showSensorError("Gyroscope"), - // cancelOnError: true, - // ), - // ); + _streamSubscriptions.add( + gyroscopeEventStream(samplingPeriod: sensorInterval).listen( + _updateOrientationFromGyroscope, + onError: (_) => _showSensorError("Gyroscope"), + cancelOnError: true, + ), + ); } void _showSensorError(String sensorName) { @@ -100,42 +99,42 @@ class _ImuWidgetState extends State { } /// Update orientation by integrating gyroscope angular velocity - // void _updateOrientationFromGyroscope(GyroscopeEvent event) { - // final now = DateTime.now(); - - // // Initialize integration time on first run - // if (_lastGyroIntegrationTime == null) { - // _lastGyroIntegrationTime = now; - // return; - // } - - // // Calculate time delta between now and last integration time (in seconds) so we know how long we've been rotating for. - // // tldr: to get orientation, we need angular velocity * time. - // final dt = now.difference(_lastGyroIntegrationTime!).inMilliseconds / 1000.0; - // _lastGyroIntegrationTime = now; + void _updateOrientationFromGyroscope(GyroscopeEvent event) { + final now = DateTime.now(); - // // Skip if dt is too large, meaning the phone has been stationary for too long in between movements. - // if (dt > 0.5) { - // return; - // } + // Initialize integration time on first run + if (_lastGyroIntegrationTime == null) { + _lastGyroIntegrationTime = now; + return; + } - // // Don't update orientation if reference point hasn't been set - // if (!_isReferenceSet) { - // return; - // } + // Calculate time delta between now and last integration time (in seconds) so we know how long we've been rotating for. + // tldr: to get orientation, we need angular velocity * time. + final dt = now.difference(_lastGyroIntegrationTime!).inMilliseconds / 1000.0; + _lastGyroIntegrationTime = now; - // // Calucluate orientation change: integrate angular velocity over time to get orientation. - // // Gyroscope values are in radians/second + // Skip if dt is too large, meaning the phone has been stationary for too long in between movements. + if (dt > 0.5) { + return; + } - // _orientationX += event.x * dt; // roll - // _orientationY += event.y * dt; // pitch - // _orientationZ += event.z * dt; // yaw + // Don't update orientation if reference point hasn't been set + if (!_isReferenceSet) { + return; + } - // // Apply small decay to prevent drift - // _orientationX *= 0.999; - // _orientationY *= 0.999; - // _orientationZ *= 0.999; - // } + // Calucluate orientation change: integrate angular velocity over time to get orientation. + // Gyroscope values are in radians/second + // 1. angular velocity to angular position (euler angles) w integration + _orientationX += event.x * dt; // roll + _orientationY += event.y * dt; // pitch + _orientationZ += event.z * dt; // yaw + + // Apply small decay to prevent drift + _orientationX *= 0.999; + _orientationY *= 0.999; + _orientationZ *= 0.999; + } /// Create a pose based on IMU accelerometer data using acceleration to get position /// The pose is added to a queue for sequential processing. @@ -197,6 +196,7 @@ class _ImuWidgetState extends State { _positionX += _velocityX * dt; _positionY += _velocityY * dt; _positionZ += _velocityZ * dt; + // print("positionX: $_positionX, positionY: $_positionY, positionZ: $_positionZ"); try { // Calculate new target position based on reference + phone displacement @@ -207,23 +207,42 @@ class _ImuWidgetState extends State { // Attempted to convert orientation values from angular velocities (yaw pitch roll) to orientation vectors // Then convert orientation vectors to quaternions and multiply them to get the new arm orientation quaternion - // final phoneQuaternion = vector_math.Quaternion.identity(); - // phoneQuaternion.setEuler(_orientationZ, _orientationY, _orientationX); // Yaw, Pitch, Roll - // final armQuaternion = ovToQuat(_referenceArmPose!.oZ, _referenceArmPose!.oY, _referenceArmPose!.oX, _referenceArmPose!.theta); - // final newArmQuaternion = armQuaternion * phoneQuaternion; - // final newOrientationVector = quatToOV(newArmQuaternion); - // final newOrientationX = newOrientationVector.x; - // final newOrientationY = newOrientationVector.y; - // final newOrientationZ = newOrientationVector.z; - + // 2. convert euler angles to quaternion + final phoneEulerAngles = EulerAngles( + _orientationX, // roll + _orientationY, // pitch + _orientationZ // yaw + ); + final phoneQuaternion = phoneEulerAngles.toQuaternion(); + + // 3: Convert latest pose to quaternion + final armOrientationVector = + OrientationVector(_referenceArmPose!.theta, _referenceArmPose!.oX, _referenceArmPose!.oY, _referenceArmPose!.oZ); + final armQuaternion = armOrientationVector.toQuaternion(); + + // 4: add quaternions (which is actually multiplying them which there is now a function for) + final newArmQuaternion = armQuaternion.mul(phoneQuaternion); + + // 5. convert the new quaternion back to orientation vector + final newOrientationVector = newArmQuaternion.toOrientationVectorRadians(); + final newOrientationX = newOrientationVector.ox; + final newOrientationY = newOrientationVector.oy; + final newOrientationZ = newOrientationVector.oz; + final newTheta = newOrientationVector.theta; + print("theta: $newTheta"); + // print("newOrientationX: $newOrientationX, newOrientationY: $newOrientationY, newOrientationZ: $newOrientationZ, newTheta: $newTheta"); + // 6. add pose to queue final newPose = Pose( - x: newX, - y: newY, - z: newZ, - theta: _referenceArmPose!.theta, - oX: _referenceArmPose!.oX, // would be newOrientationX if math was correct - oY: _referenceArmPose!.oY, - oZ: _referenceArmPose!.oZ, + // x: newX, + // y: newY, + // z: newZ, + x: _referenceArmPose!.x, + y: _referenceArmPose!.y, + z: _referenceArmPose!.z, + theta: newTheta, + oX: newOrientationX, + oY: newOrientationY, + oZ: newOrientationZ, ); if (_poseQueue.isNotEmpty) { @@ -241,7 +260,7 @@ class _ImuWidgetState extends State { }); // Add pose to queue - _addPoseToQueue(newPose, 0); + _addPoseToQueue(newPose, 5); if (!_isProcessingQueue) { _executePoseFromQueue(); @@ -308,10 +327,10 @@ class _ImuWidgetState extends State { _lastIntegrationTime = null; // Zero out orientation tracking - // _orientationX = 0.0; - // _orientationY = 0.0; - // _orientationZ = 0.0; - // _lastGyroIntegrationTime = null; + _orientationX = 0.0; + _orientationY = 0.0; + _orientationZ = 0.0; + _lastGyroIntegrationTime = null; // Clear pose queue and reset counter _poseQueue.clear(); @@ -407,7 +426,14 @@ class _ImuWidgetState extends State { }); }, child: Text("Stop"), - ) + ), + ElevatedButton( + onPressed: () async { + await widget.arm.moveToJointPositions([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]); + await widget.arm.moveToPosition(Pose(x: 300, y: 0, z: 100, oX: 0, oY: 0, oZ: -1, theta: 0)); + }, + child: Text("reset position"), + ), ], ); } @@ -417,158 +443,158 @@ class _ImuWidgetState extends State { /// Translated from Go spatialmath code - OrientationVector.Quaternion() /// OX, OY, OZ represent a point on the unit sphere where the end effector is pointing /// Theta is rotation around that pointing axis - vector_math.Quaternion ovToQuat(double oX, double oY, double oZ, double theta) { - const double defaultAngleEpsilon = 1e-4; - - // Normalize the orientation vector - final norm = math.sqrt(oX * oX + oY * oY + oZ * oZ); - double normOX = oX; - double normOY = oY; - double normOZ = oZ; - - if (norm == 0.0) { - // Default orientation: pointing up along Z axis - normOX = 0.0; - normOY = 0.0; - normOZ = -1.0; - } else { - normOX /= norm; - normOY /= norm; - normOZ /= norm; - } - - // acos(OZ) ranges from 0 (north pole) to pi (south pole) - final lat = math.acos(normOZ.clamp(-1.0, 1.0)); - - // If we're pointing at the Z axis then lon is 0, theta is the OV theta - double lon = 0.0; - - if (1 - normOZ.abs() > defaultAngleEpsilon) { - // If we are not at a pole, we need the longitude - lon = math.atan2(normOY, normOX); - } - - // Use ZYZ Euler angles to create quaternion - // This matches: mgl64.AnglesToQuat(lon, lat, theta, mgl64.ZYZ) - return _eulerZYZToQuat(lon, lat, theta); - } - - /// Convert ZYZ Euler angles to quaternion - /// Manually composing Q1(Z by z1) * Q2(Y by y) * Q3(Z by z2) - vector_math.Quaternion _eulerZYZToQuat(double z1, double y, double z2) { - // Create three rotation quaternions and compose them - // Q1: Rotation around Z axis by z1 - final q1 = vector_math.Quaternion.axisAngle(vector_math.Vector3(0, 0, 1), z1); - - // Q2: Rotation around Y axis by y - final q2 = vector_math.Quaternion.axisAngle(vector_math.Vector3(0, 1, 0), y); - - // Q3: Rotation around Z axis by z2 - final q3 = vector_math.Quaternion.axisAngle(vector_math.Vector3(0, 0, 1), z2); - - // Compose: Q = Q1 * Q2 * Q3 (intrinsic rotations) - final result = q1 * q2 * q3; - - return result; - } - - /// Converts a unit quaternion (q) to an OrientationVector. - /// Converted from go code to flutter using Gemini - Orientation_OrientationVectorRadians quatToOV(vector_math.Quaternion q) { - double orientationVectorPoleRadius = 0.0001; - double defaultAngleEpsilon = 1e-4; - // Define initial axes as pure quaternions (Real/W=0) - // xAxis: (0, -1, 0, 0) -> x=-1, y=0, z=0, w=0 - final vector_math.Quaternion xAxis = vector_math.Quaternion(0.0, -1.0, 0.0, 0.0); - // zAxis: (0, 0, 0, 1) -> x=0, y=0, z=1, w=0 - final vector_math.Quaternion zAxis = vector_math.Quaternion(0.0, 0.0, 0.0, 1.0); - - final ov = Orientation_OrientationVectorRadians(); - - // Get the transform of our +X and +Z points (Quaternion rotation formula: q * v * q_conj) - final vector_math.Quaternion newX = q * xAxis * q.conjugated(); - final vector_math.Quaternion newZ = q * zAxis * q.conjugated(); - - // Set the direction vector (OX, OY, OZ) from the rotated Z-axis (Imag, Jmag, Kmag components) - ov.x = newZ.x; - ov.y = newZ.y; - ov.z = newZ.z; - - // Calculate the roll angle (Theta) - - // Check if we are near the poles (i.e., newZ.z/Kmag is close to 1 or -1) - if (1 - (ov.z.abs()) > orientationVectorPoleRadius) { - // General Case: Not Near the Pole - - // Vector3 versions of the rotated axes - final vector_math.Vector3 v1 = vector_math.Vector3(newZ.x, newZ.y, newZ.z); // Local Z - final vector_math.Vector3 v2 = vector_math.Vector3(newX.x, newX.y, newX.z); // Local X - final vector_math.Vector3 globalZ = vector_math.Vector3(0.0, 0.0, 1.0); // Global Z - - // Normal to the local-x, local-z plane - final vector_math.Vector3 norm1 = v1.cross(v2); - - // Normal to the global-z, local-z plane - final vector_math.Vector3 norm2 = v1.cross(globalZ); - - // Find the angle (theta) between the two planes (using the angle between their normals) - final double denominator = norm1.length * norm2.length; - final double cosTheta = norm1.dot(norm2) / denominator; // Avoid division by zero, default to 1 (0 angle) - - // Clamp for float error - double clampedCosTheta = cosTheta.clamp(-1.0, 1.0); + // vector_math.Quaternion ovToQuat(double oX, double oY, double oZ, double theta) { + // const double defaultAngleEpsilon = 1e-4; + + // // Normalize the orientation vector + // final norm = math.sqrt(oX * oX + oY * oY + oZ * oZ); + // double normOX = oX; + // double normOY = oY; + // double normOZ = oZ; + + // if (norm == 0.0) { + // // Default orientation: pointing up along Z axis + // normOX = 0.0; + // normOY = 0.0; + // normOZ = -1.0; + // } else { + // normOX /= norm; + // normOY /= norm; + // normOZ /= norm; + // } - final double theta = math.acos(clampedCosTheta); + // // acos(OZ) ranges from 0 (north pole) to pi (south pole) + // final lat = math.acos(normOZ.clamp(-1.0, 1.0)); - if (theta.abs() > orientationVectorPoleRadius) { - // Determine directionality of the angle (sign of theta) + // // If we're pointing at the Z axis then lon is 0, theta is the OV theta + // double lon = 0.0; - // Axis is the new Z-axis (ov.OX, ov.OY, ov.OZ) - final vector_math.Vector3 axis = vector_math.Vector3(ov.x, ov.y, ov.z); - // Create a rotation quaternion for rotation by -theta around the new Z-axis - final vector_math.Quaternion q2 = vector_math.Quaternion.axisAngle(axis, -theta); + // if (1 - normOZ.abs() > defaultAngleEpsilon) { + // // If we are not at a pole, we need the longitude + // lon = math.atan2(normOY, normOX); + // } - // Apply q2 rotation to the original Z-axis (0, 0, 0, 1) - final vector_math.Quaternion testZQuat = q2 * zAxis * q2.conjugated(); - final vector_math.Vector3 testZVector = vector_math.Vector3(testZQuat.x, testZQuat.y, testZQuat.z); + // // Use ZYZ Euler angles to create quaternion + // // This matches: mgl64.AnglesToQuat(lon, lat, theta, mgl64.ZYZ) + // return _eulerZYZToQuat(lon, lat, theta); + // } - // Find the normal of the plane defined by v1 (local Z) and testZ - final vector_math.Vector3 norm3 = v1.cross(testZVector); + // /// Convert ZYZ Euler angles to quaternion + // /// Manually composing Q1(Z by z1) * Q2(Y by y) * Q3(Z by z2) + // vector_math.Quaternion _eulerZYZToQuat(double z1, double y, double z2) { + // // Create three rotation quaternions and compose them + // // Q1: Rotation around Z axis by z1 + // final q1 = vector_math.Quaternion.axisAngle(vector_math.Vector3(0, 0, 1), z1); - final double norm1Len = norm1.length; - final double norm3Len = norm3.length; + // // Q2: Rotation around Y axis by y + // final q2 = vector_math.Quaternion.axisAngle(vector_math.Vector3(0, 1, 0), y); - final double cosTest = norm1.dot(norm3) / (norm1Len * norm3Len); + // // Q3: Rotation around Z axis by z2 + // final q3 = vector_math.Quaternion.axisAngle(vector_math.Vector3(0, 0, 1), z2); - // Check if norm1 and norm3 are coplanar (angle close to 0) - if (1.0 - cosTest < defaultAngleEpsilon * defaultAngleEpsilon) { - ov.theta = -theta; - } else { - ov.theta = theta; - } - } else { - ov.theta = 0.0; - } - } else { - // Special Case: Near the Pole (Z-axis is up or down) + // // Compose: Q = Q1 * Q2 * Q3 (intrinsic rotations) + // final result = q1 * q2 * q3; - // Use Atan2 on the rotated X-axis components (Jmag and Imag, or y and x in Dart) - // -math.Atan2(newX.Jmag, -newX.Imag) -> Dart: -math.atan2(newX.y, -newX.x) - ov.theta = -math.atan2(newX.y, -newX.x); + // return result; + // } - if (newZ.z < 0) { - // If pointing along the negative Z-axis (ov.OZ < 0) - // -math.Atan2(newX.Jmag, newX.Imag) -> Dart: -math.atan2(newX.y, newX.x) - ov.theta = -math.atan2(newX.y, newX.x); - } - } + // /// Converts a unit quaternion (q) to an OrientationVector. + // /// Converted from go code to flutter using Gemini + // Orientation_OrientationVectorRadians quatToOV(vector_math.Quaternion q) { + // double orientationVectorPoleRadius = 0.0001; + // double defaultAngleEpsilon = 1e-4; + // // Define initial axes as pure quaternions (Real/W=0) + // // xAxis: (0, -1, 0, 0) -> x=-1, y=0, z=0, w=0 + // final vector_math.Quaternion xAxis = vector_math.Quaternion(0.0, -1.0, 0.0, 0.0); + // // zAxis: (0, 0, 0, 1) -> x=0, y=0, z=1, w=0 + // final vector_math.Quaternion zAxis = vector_math.Quaternion(0.0, 0.0, 0.0, 1.0); + + // final ov = Orientation_OrientationVectorRadians(); + + // // Get the transform of our +X and +Z points (Quaternion rotation formula: q * v * q_conj) + // final vector_math.Quaternion newX = q * xAxis * q.conjugated(); + // final vector_math.Quaternion newZ = q * zAxis * q.conjugated(); + + // // Set the direction vector (OX, OY, OZ) from the rotated Z-axis (Imag, Jmag, Kmag components) + // ov.x = newZ.x; + // ov.y = newZ.y; + // ov.z = newZ.z; + + // // Calculate the roll angle (Theta) + + // // Check if we are near the poles (i.e., newZ.z/Kmag is close to 1 or -1) + // if (1 - (ov.z.abs()) > orientationVectorPoleRadius) { + // // General Case: Not Near the Pole + + // // Vector3 versions of the rotated axes + // final vector_math.Vector3 v1 = vector_math.Vector3(newZ.x, newZ.y, newZ.z); // Local Z + // final vector_math.Vector3 v2 = vector_math.Vector3(newX.x, newX.y, newX.z); // Local X + // final vector_math.Vector3 globalZ = vector_math.Vector3(0.0, 0.0, 1.0); // Global Z + + // // Normal to the local-x, local-z plane + // final vector_math.Vector3 norm1 = v1.cross(v2); + + // // Normal to the global-z, local-z plane + // final vector_math.Vector3 norm2 = v1.cross(globalZ); + + // // Find the angle (theta) between the two planes (using the angle between their normals) + // final double denominator = norm1.length * norm2.length; + // final double cosTheta = norm1.dot(norm2) / denominator; // Avoid division by zero, default to 1 (0 angle) + + // // Clamp for float error + // double clampedCosTheta = cosTheta.clamp(-1.0, 1.0); + + // final double theta = math.acos(clampedCosTheta); + + // if (theta.abs() > orientationVectorPoleRadius) { + // // Determine directionality of the angle (sign of theta) + + // // Axis is the new Z-axis (ov.OX, ov.OY, ov.OZ) + // final vector_math.Vector3 axis = vector_math.Vector3(ov.x, ov.y, ov.z); + // // Create a rotation quaternion for rotation by -theta around the new Z-axis + // final vector_math.Quaternion q2 = vector_math.Quaternion.axisAngle(axis, -theta); + + // // Apply q2 rotation to the original Z-axis (0, 0, 0, 1) + // final vector_math.Quaternion testZQuat = q2 * zAxis * q2.conjugated(); + // final vector_math.Vector3 testZVector = vector_math.Vector3(testZQuat.x, testZQuat.y, testZQuat.z); + + // // Find the normal of the plane defined by v1 (local Z) and testZ + // final vector_math.Vector3 norm3 = v1.cross(testZVector); + + // final double norm1Len = norm1.length; + // final double norm3Len = norm3.length; + + // final double cosTest = norm1.dot(norm3) / (norm1Len * norm3Len); + + // // Check if norm1 and norm3 are coplanar (angle close to 0) + // if (1.0 - cosTest < defaultAngleEpsilon * defaultAngleEpsilon) { + // ov.theta = -theta; + // } else { + // ov.theta = theta; + // } + // } else { + // ov.theta = 0.0; + // } + // } else { + // // Special Case: Near the Pole (Z-axis is up or down) + + // // Use Atan2 on the rotated X-axis components (Jmag and Imag, or y and x in Dart) + // // -math.Atan2(newX.Jmag, -newX.Imag) -> Dart: -math.atan2(newX.y, -newX.x) + // ov.theta = -math.atan2(newX.y, -newX.x); + + // if (newZ.z < 0) { + // // If pointing along the negative Z-axis (ov.OZ < 0) + // // -math.Atan2(newX.Jmag, newX.Imag) -> Dart: -math.atan2(newX.y, newX.x) + // ov.theta = -math.atan2(newX.y, newX.x); + // } + // } - // Handle IEEE -0.0 for consistency - if (ov.theta == -0.0) { - ov.theta = 0.0; - } - return ov; - } + // // Handle IEEE -0.0 for consistency + // if (ov.theta == -0.0) { + // ov.theta = 0.0; + // } + // return ov; + // } @override void dispose() { diff --git a/example/viam_example_app/spatialmath/common.dart b/example/viam_example_app/lib/spatialmath/common.dart similarity index 100% rename from example/viam_example_app/spatialmath/common.dart rename to example/viam_example_app/lib/spatialmath/common.dart diff --git a/example/viam_example_app/spatialmath/euler_angles.dart b/example/viam_example_app/lib/spatialmath/euler_angles.dart similarity index 100% rename from example/viam_example_app/spatialmath/euler_angles.dart rename to example/viam_example_app/lib/spatialmath/euler_angles.dart diff --git a/example/viam_example_app/spatialmath/orientation_vector.dart b/example/viam_example_app/lib/spatialmath/orientation_vector.dart similarity index 100% rename from example/viam_example_app/spatialmath/orientation_vector.dart rename to example/viam_example_app/lib/spatialmath/orientation_vector.dart diff --git a/example/viam_example_app/spatialmath/quaternion.dart b/example/viam_example_app/lib/spatialmath/quaternion.dart similarity index 100% rename from example/viam_example_app/spatialmath/quaternion.dart rename to example/viam_example_app/lib/spatialmath/quaternion.dart diff --git a/example/viam_example_app/spatialmath/spatial_math.dart b/example/viam_example_app/lib/spatialmath/spatial_math.dart similarity index 100% rename from example/viam_example_app/spatialmath/spatial_math.dart rename to example/viam_example_app/lib/spatialmath/spatial_math.dart diff --git a/example/viam_example_app/pubspec.yaml b/example/viam_example_app/pubspec.yaml index 60f36c581c2..5a72770a9aa 100644 --- a/example/viam_example_app/pubspec.yaml +++ b/example/viam_example_app/pubspec.yaml @@ -11,6 +11,7 @@ dependencies: sdk: flutter flutter_dotenv: ^5.1.0 sensors_plus: ^7.0.0 + arkit_plugin: ^1.2.1 vector_math: ^2.2.0 viam_sdk: path: ../../