Adam
cc19e102e8
Implements professional-grade trajectory planning and execution capabilities
for industrial robotics applications. Adds velocity profiling, geometric
motion primitives, path blending, and coordinate frame transformations.
Features Added:
- Velocity profiling (trapezoidal and S-curve profiles)
- Geometric motion primitives (arc, circle, spiral)
- Path blending with cubic Hermite spline interpolation
- Coordinate transformations (BASE/WORLD/TOOL/WORK frames)
New API Methods (MotionAPI):
- generate_velocity_profile(trajectory, max_velocity, max_acceleration, profile)
- generate_arc(center, radius, start_angle, end_angle, steps, plane)
- generate_circle(center, radius, steps, plane)
- generate_spiral(center, start_radius, end_radius, pitch, revolutions, steps, plane, axis)
- blend_trajectories(traj1, traj2, blend_radius, blend_steps)
- transform_coordinates(pose, from_frame, to_frame, frame_offset)
Helper Functions:
- _calculate_distance() - Euclidean distance between waypoints
- _trapezoidal_profile() - Bang-bang velocity control
- _s_curve_profile() - Jerk-limited smooth profiles
- _find_blend_point() - Locate blend zone boundaries
- _cubic_blend() - Cubic Hermite spline interpolation
Examples Created (examples/advanced_motion/):
- 01_velocity_profiles.py (234 lines) - Trapezoidal vs S-curve profiling
- 02_geometric_primitives.py (225 lines) - Arc, circle, spiral patterns
- 03_path_blending.py (253 lines) - Smooth trajectory transitions
- 04_coordinate_transforms.py (284 lines) - Frame transformations
- 05_combined_motion.py (336 lines) - Complete production application
- README.md (584 lines) - Comprehensive documentation
Documentation:
- PHASE_4_SUMMARY.md - Detailed implementation documentation
- Updated ROADMAP.md to mark Phase 4 complete
- Comprehensive API documentation in examples/advanced_motion/README.md
Files Modified:
- src/RSIPI/motion_api.py (~550 lines added)
- ROADMAP.md (updated Phase 4 status)
Files Created:
- PHASE_4_SUMMARY.md
- examples/advanced_motion/ (6 new files, 1,916 total lines)
Statistics:
- New API methods: 5 public methods + 4 helper functions
- Example code: ~1,332 lines
- Documentation: ~584 lines
- Total additions: ~2,466 lines
Production Applications:
- Drilling and milling (expanding/contracting spirals)
- Assembly (circular insertion, smooth approaches)
- Inspection (spiral scanning, circular features)
- Welding/coating (continuous beads, smooth transitions)
- Pick and place (optimized cycles, blended paths)
Phase 4 Status: ✅ COMPLETE
Date: January 17, 2026
282 lines
9.8 KiB
Python
282 lines
9.8 KiB
Python
"""
|
|
Path Blending Example
|
|
|
|
Demonstrates smooth trajectory transitions using cubic interpolation for
|
|
eliminating stop-and-go motion at trajectory boundaries.
|
|
|
|
Usage:
|
|
python 03_path_blending.py --config RSI_EthernetConfig.xml
|
|
"""
|
|
|
|
import argparse
|
|
import logging
|
|
from RSIPI import RSIAPI
|
|
|
|
logging.basicConfig(
|
|
level=logging.INFO,
|
|
format='%(asctime)s - %(levelname)s - %(message)s'
|
|
)
|
|
|
|
|
|
def path_blending_example(config_file: str) -> None:
|
|
"""
|
|
Demonstrate path blending for smooth trajectory transitions.
|
|
|
|
Args:
|
|
config_file: Path to RSI configuration XML file
|
|
"""
|
|
api = RSIAPI(config_file)
|
|
|
|
try:
|
|
logging.info("Starting RSI communication...")
|
|
api.start()
|
|
logging.info("✅ RSI started successfully")
|
|
|
|
# ==================================================
|
|
# Example 1: Sharp Corner vs Blended Corner
|
|
# ==================================================
|
|
logging.info("\n" + "=" * 60)
|
|
logging.info("Example 1: Sharp Corner vs Blended Corner")
|
|
logging.info("=" * 60)
|
|
|
|
# Define three points forming a right angle
|
|
p0 = {"X": 100, "Y": 0, "Z": 500}
|
|
p1 = {"X": 200, "Y": 0, "Z": 500} # Corner point
|
|
p2 = {"X": 200, "Y": 100, "Z": 500}
|
|
|
|
# Generate two separate trajectories
|
|
traj1 = api.motion.generate_trajectory(p0, p1, steps=50)
|
|
traj2 = api.motion.generate_trajectory(p1, p2, steps=50)
|
|
|
|
logging.info("\nWithout blending:")
|
|
logging.info(" - Robot will stop at corner point")
|
|
logging.info(" - Visible acceleration/deceleration")
|
|
logging.info(" - Less smooth motion")
|
|
|
|
# Execute sharp corner (no blending)
|
|
logging.info("\nExecuting sharp corner motion...")
|
|
api.motion.execute_trajectory(traj1, space='cartesian', rate=0.02)
|
|
api.motion.execute_trajectory(traj2, space='cartesian', rate=0.02)
|
|
logging.info("✅ Sharp corner complete")
|
|
|
|
# Return to start
|
|
api.motion.execute_trajectory(
|
|
api.motion.generate_trajectory(p2, p0, steps=50),
|
|
space='cartesian',
|
|
rate=0.02
|
|
)
|
|
|
|
# Now execute with blending
|
|
logging.info("\nWith blending:")
|
|
logging.info(" - Smooth transition through corner")
|
|
logging.info(" - No visible stop at corner point")
|
|
logging.info(" - Continuous velocity")
|
|
|
|
blended = api.motion.blend_trajectories(
|
|
traj1,
|
|
traj2,
|
|
blend_radius=20.0, # Start blending 20mm before corner
|
|
blend_steps=20
|
|
)
|
|
|
|
logging.info(f"\nBlended trajectory: {len(blended)} waypoints")
|
|
logging.info(f"Original: {len(traj1) + len(traj2)} waypoints")
|
|
logging.info(f"Blend zone: 20.0 mm radius")
|
|
|
|
logging.info("Executing blended corner motion...")
|
|
api.motion.execute_trajectory(blended, space='cartesian', rate=0.02)
|
|
logging.info("✅ Blended corner complete")
|
|
|
|
# ==================================================
|
|
# Example 2: Multiple Blend Zones (Continuous Path)
|
|
# ==================================================
|
|
logging.info("\n" + "=" * 60)
|
|
logging.info("Example 2: Continuous Path with Multiple Blends")
|
|
logging.info("=" * 60)
|
|
|
|
# Define waypoints for a square pattern
|
|
waypoints = [
|
|
{"X": 100, "Y": 0, "Z": 500},
|
|
{"X": 150, "Y": 0, "Z": 500},
|
|
{"X": 150, "Y": 50, "Z": 500},
|
|
{"X": 100, "Y": 50, "Z": 500},
|
|
{"X": 100, "Y": 0, "Z": 500} # Return to start
|
|
]
|
|
|
|
logging.info(f"Square pattern with {len(waypoints)} waypoints")
|
|
|
|
# Generate segments
|
|
segments = []
|
|
for i in range(len(waypoints) - 1):
|
|
segment = api.motion.generate_trajectory(
|
|
waypoints[i],
|
|
waypoints[i + 1],
|
|
steps=30
|
|
)
|
|
segments.append(segment)
|
|
|
|
logging.info(f"Generated {len(segments)} path segments")
|
|
|
|
# Blend all segments together
|
|
logging.info("Blending all corners...")
|
|
blended_path = segments[0]
|
|
|
|
for i in range(1, len(segments)):
|
|
blended_path = api.motion.blend_trajectories(
|
|
blended_path,
|
|
segments[i],
|
|
blend_radius=10.0,
|
|
blend_steps=15
|
|
)
|
|
logging.info(f" Blended corner {i}/{len(segments)-1}")
|
|
|
|
logging.info(f"\nFinal blended path: {len(blended_path)} waypoints")
|
|
logging.info("Executing continuous square pattern...")
|
|
api.motion.execute_trajectory(blended_path, space='cartesian', rate=0.02)
|
|
logging.info("✅ Continuous square complete")
|
|
|
|
# ==================================================
|
|
# Example 3: Different Blend Radii
|
|
# ==================================================
|
|
logging.info("\n" + "=" * 60)
|
|
logging.info("Example 3: Effect of Different Blend Radii")
|
|
logging.info("=" * 60)
|
|
|
|
# Same trajectories as Example 1
|
|
traj1 = api.motion.generate_trajectory(p0, p1, steps=50)
|
|
traj2 = api.motion.generate_trajectory(p1, p2, steps=50)
|
|
|
|
blend_radii = [5.0, 15.0, 30.0]
|
|
|
|
for radius in blend_radii:
|
|
logging.info(f"\n--- Blend Radius: {radius} mm ---")
|
|
|
|
blended = api.motion.blend_trajectories(
|
|
traj1,
|
|
traj2,
|
|
blend_radius=radius,
|
|
blend_steps=20
|
|
)
|
|
|
|
logging.info(f"Blend radius: {radius} mm")
|
|
logging.info(f"Blended waypoints: {len(blended)}")
|
|
|
|
if radius < 10:
|
|
logging.info("Effect: Tighter blend, closer to sharp corner")
|
|
elif radius < 20:
|
|
logging.info("Effect: Moderate blend, balanced smoothness")
|
|
else:
|
|
logging.info("Effect: Wide blend, very smooth but cuts corner more")
|
|
|
|
logging.info(f"Executing blend with radius={radius}mm...")
|
|
api.motion.execute_trajectory(blended, space='cartesian', rate=0.02)
|
|
logging.info(f"✅ Blend radius {radius}mm complete")
|
|
|
|
# Return to start
|
|
api.motion.execute_trajectory(
|
|
api.motion.generate_trajectory(p2, p0, steps=50),
|
|
space='cartesian',
|
|
rate=0.02
|
|
)
|
|
|
|
# ==================================================
|
|
# Example 4: Blending with Different Orientations
|
|
# ==================================================
|
|
logging.info("\n" + "=" * 60)
|
|
logging.info("Example 4: Blending Trajectories with Orientation Changes")
|
|
logging.info("=" * 60)
|
|
|
|
# Define points with different orientations
|
|
p0_rot = {"X": 100, "Y": 0, "Z": 500, "A": 0, "B": 0, "C": 0}
|
|
p1_rot = {"X": 150, "Y": 0, "Z": 500, "A": 0, "B": 0, "C": 45}
|
|
p2_rot = {"X": 150, "Y": 50, "Z": 500, "A": 0, "B": 0, "C": 90}
|
|
|
|
traj1_rot = api.motion.generate_trajectory(p0_rot, p1_rot, steps=50)
|
|
traj2_rot = api.motion.generate_trajectory(p1_rot, p2_rot, steps=50)
|
|
|
|
logging.info("Trajectory with orientation change:")
|
|
logging.info(f" Start: C = 0°")
|
|
logging.info(f" Corner: C = 45°")
|
|
logging.info(f" End: C = 90°")
|
|
|
|
blended_rot = api.motion.blend_trajectories(
|
|
traj1_rot,
|
|
traj2_rot,
|
|
blend_radius=15.0,
|
|
blend_steps=20
|
|
)
|
|
|
|
logging.info(f"\nBlending also smooths orientation transitions")
|
|
logging.info(f"Blended waypoints: {len(blended_rot)}")
|
|
|
|
logging.info("Executing blended motion with rotation...")
|
|
api.motion.execute_trajectory(blended_rot, space='cartesian', rate=0.02)
|
|
logging.info("✅ Blended rotation complete")
|
|
|
|
# ==================================================
|
|
# Application Examples
|
|
# ==================================================
|
|
logging.info("\n" + "=" * 60)
|
|
logging.info("Application Examples")
|
|
logging.info("=" * 60)
|
|
|
|
logging.info("\nPick and Place Applications:")
|
|
logging.info(" - Smooth transitions between pick/place points")
|
|
logging.info(" - Reduced cycle time by eliminating stops")
|
|
logging.info(" - Lower mechanical stress on robot")
|
|
|
|
logging.info("\nWelding/Gluing Applications:")
|
|
logging.info(" - Continuous bead at corners without stop marks")
|
|
logging.info(" - Consistent material deposition rate")
|
|
logging.info(" - Professional finish quality")
|
|
|
|
logging.info("\nMachining Applications:")
|
|
logging.info(" - Smooth tool paths without witness marks")
|
|
logging.info(" - Reduced vibration and tool wear")
|
|
logging.info(" - Better surface finish")
|
|
|
|
logging.info("\nPainting/Coating Applications:")
|
|
logging.info(" - Even coating thickness at corners")
|
|
logging.info(" - No overspray from deceleration")
|
|
logging.info(" - Faster throughput")
|
|
|
|
except KeyboardInterrupt:
|
|
logging.warning("\n⚠️ Interrupted by user")
|
|
|
|
except Exception as e:
|
|
logging.error(f"❌ Error during path blending: {e}")
|
|
|
|
finally:
|
|
logging.info("Stopping RSI communication...")
|
|
api.stop()
|
|
logging.info("✅ API stopped successfully")
|
|
|
|
|
|
def main():
|
|
"""Main entry point."""
|
|
parser = argparse.ArgumentParser(description='Path Blending Example')
|
|
parser.add_argument(
|
|
'--config',
|
|
type=str,
|
|
default='RSI_EthernetConfig.xml',
|
|
help='Path to RSI configuration file'
|
|
)
|
|
|
|
args = parser.parse_args()
|
|
|
|
logging.info("=" * 60)
|
|
logging.info("RSIPI - Path Blending Example")
|
|
logging.info("=" * 60)
|
|
logging.info(f"Config: {args.config}")
|
|
logging.info("=" * 60)
|
|
|
|
path_blending_example(args.config)
|
|
|
|
logging.info("=" * 60)
|
|
logging.info("Example complete!")
|
|
logging.info("=" * 60)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|