@Snoopsawg

Simulating the triple pendulum

import sympy as sp import inspectt=sp.symbols('t')theta1=sp.Function('theta1')(t)theta2=sp.Function('theta2')(t)theta3=sp.Function('theta3')(t)L1, L2, L3 = sp.symbols('L1 L2 L3')m1, m2, m3 = sp.symbols('m1 m2 m3')x1= L1*sp.sin(theta1)y1= -L1*sp.cos(theta1)x2= x1 + L2*sp.sin(theta2)y2= y1 - L2*sp.cos(theta2)x3= x2 + L3*sp.sin(theta3)y3= y2 - L3*sp.cos(theta3)T1,T2,T3 = sp.symbols('T1 T2 T3')V1,V2,V3 = sp.symbols('V1 V2 V3')x1dd= sp.diff(x1,t,2)x2dd= sp.diff(x2,t,2)x3dd= sp.diff(x3,t,2)y1dd= sp.diff(y1,t,2)y2dd= sp.diff(y2,t,2)y3dd= sp.diff(y3,t,2)vx1=sp.diff(x1,t)vy1=sp.diff(y1,t)vx2=sp.diff(x2,t)vy2=sp.diff(y2,t)vx3=sp.diff(x3,t)vy3=sp.diff(y3,t)T = (sp.Rational(1,2)*m1*(vx1**2 + vy1**2) + sp.Rational(1,2)*m2*(vx2**2 + vy2**2) + sp.Rational(1,2)*m3*(vx3**2 + vy3**2))g = sp.symbols('g')V = m1*g*y1 + m2*g*y2 + m3*g*y3 Lagrangian = T - Vprint("Getting Lagrangian!!!")Lagrangian = sp.simplify(Lagrangian)print(Lagrangian)print("Hey, we got the Lagrangian!")#Euler-Lagrange equations:Euler1= sp.diff(sp.diff(Lagrangian, sp.diff(theta1,t)), t) - sp.diff(Lagrangian, theta1)Euler2= sp.diff(sp.diff(Lagrangian, sp.diff(theta2,t)), t) - sp.diff(Lagrangian, theta2)Euler3= sp.diff(sp.diff(Lagrangian, sp.diff(theta3,t)), t) - sp.diff(Lagrangian, theta3)theta1dd=sp.diff(theta1,t,2)theta2dd=sp.diff(theta2,t,2)theta3dd=sp.diff(theta3,t,2)derivs=[theta1dd, theta2dd, theta3dd]solution=sp.solve((Euler1, Euler2, Euler3), derivs, dict=True)alpha1 = solution[0][derivs[0]]alpha2 = solution[0][derivs[1]]alpha3 = solution[0][derivs[2]]#now i want to simplify the solution and then convert it to a function that can be used in numerical simulations.print("alpha1=", alpha1)print("******************************************************************************************")print("alpha2=", alpha2)print("******************************************************************************************")print("alpha3=", alpha3)accel_func = sp.lambdify( (theta1, theta2, theta3, sp.diff(theta1,t), sp.diff(theta2,t), sp.diff(theta3,t), L1, L2, L3, m1, m2, m3, g), (solution[0][theta1dd], solution[0][theta2dd], solution[0][theta3dd]), "numpy")print("Writing equations with proper NumPy math imports...")with open("pendulum_equations.py", "w") as f: f.write("import numpy as np\n") # This injects the missing sin and cos tools directly into the math module! f.write("from numpy import sin, cos\n\n") f.write(inspect.getsource(accel_func))print("Successfully saved math script to 'pendulum_equations.py'!")# ... Your existing derivation code that creates 'accel' ...# #x1dd= -L1*((theta1.diff()*theta1.diff())*sp.sin(theta1)) + (theta1.diff().diff()*(L1*sp.cos(theta1)))# x2dd=x1dd - (L2*theta2.diff()*theta2.diff()*sp.sin(theta2)) + (theta2.diff().diff()*L2*sp.cos(theta2))# x3dd=x2dd - (theta3.diff()*theta3.diff()*L3*sp.sin(theta3)) + (theta3.diff().diff()*sp.sin(theta3)*L3)# y1dd= (theta1.diff()*theta1.diff()*L1*sp.cos(theta1))+(theta1.diff().diff()*L1*sp.sin(theta1))# y2dd=y1dd + (theta2.diff()*theta2.diff()*L2*sp.cos(theta2))+(theta2.diff().diff()*L2*sp.sin(theta2))# y3dd=y2dd + (theta3.diff()*theta3.diff()*L3*sp.cos(theta3))+(theta3.diff().diff()*L3*sp.sin(theta3))# vx1=sp.diff(x1,t)# vy1=sp.diff(y1,t)# vx2=sp.diff(x2,t)# vy2=sp.diff(y2,t)# vx3=sp.diff(x3,t)# vy3=sp.diff(y3,t)# T= # #Equation1 # eq1 =sp.Eq(m1*x1dd, (-T1*sp.sin(theta1))+(T2*sp.sin(theta2)))# #Equation2# eq2 = sp.Eq(m1*y1dd, (T1*sp.cos(theta1))-(T2*sp.cos(theta2))-(m1*9.81))# #Equation3# eq3 = sp.Eq(m2*x2dd, (-T2*sp.sin(theta2))+(T3*sp.sin(theta3)))# #Equation4# eq4 = sp.Eq(m2*y2dd, (T2*sp.cos(theta2))-(T3*sp.cos(theta3))-(m2*9.81))# #Equation5# eq5 = sp.Eq(m3*x3dd, (-T3*sp.sin(theta3))# #Equation6# eq6 = sp.Eq(m3*y3dd, (T3*sp.cos(theta3)) - (m3*9.81))# sp.solve([eq1, eq2, eq3, eq4, eq5, eq6], (theta1.diff().diff(), theta2.diff().diff(), theta3.diff().diff(), T1, T2, T3)), dict=True

TROUBLESHOOTING!

#Devlog for 12/06/26 for Project Handy
#11:04, I've been trying to troubleshoot this thing for the last 45 minutes now; nothing new is happening.
#11:09, I reconnected the servo directly to the Pi and it works fine, the pinout it servo orange to pin 16, servo red to pin 2, and servo brown to pin 6, so the problem is definitely with the breadboard wiring
#NOTE: still need to fix some micromovements, but the servo is moving back and forth which is good, so I just need to fix the micromovements and then connect the other servos.... if that even works out
#11:34: I think I'm gonna wait for the power module to arrive before I try to connect the other servos, I don't want to deal with the messy wiring anymore, hopefully the power module will make things easier and less prone to errors

#This is a Devlog for 12/06/26 for Project Handy

#Okay so right now I have one servo motor attached to the Raspberry Pi:
#Brown –> Pin 6 (Ground)
#Red –> Pin 2 (5V)
#Orange –> Pin 12 (GPIO 18)

#Uhhhh, okay the Raspi is now saying that the connection is being refused???

#12:27: trying to figure out the problem with the sshing thingy but I’m not sure, this doesn’t usually happen

#12:31: Okay I fixed the problem I think we are good hpprayayayay

#12:37 I ran yesterday’s code and it worked, the servo motor is moving back and forth, but the micromovements still aren’t there

#12:40 I think I know what the problem is, the servo motor is moving back and forth but it’s not doing the micromovements because the code is only telling it to move to 0 degrees and then to 180 degrees, so I need to change the code to make it move to different angles in between

#I think I’m gonna try and troubleshoot my code and see what’s up

#12:47 Okay I figured out that maybe the code was erasing micromovements because it was rounding the angle to WHOLE degrees, so I changed the code to round to 2 decimal places instead of whole numbers, hopefully that will fix the problem

#12:52 I ran the code and it seems to be working better, the servo motor is moving to different angles instead of just 0 and 180, but I still need to test it more to see if the micromovements are there and connect the other servos

#I think I’m gonna try and connect the other servos now and see if they are working properly, hopefully they will be

#So I don’t have a USB board so I’m gonna have to cut up a USB-A cable

#1:00 I cut up the USB-B cable and I have the wires ready, now I just need to connect them to the breadboard and then to the Raspberry Pi

#I have red, blue, green, white cables; usually there’s black instead of blue for the ground GND but I’m gonna assume blue is just ground GND (which is what Google says too)

#1:15 I connected the red wire to 5V, the blue wire to GND, and the green and white wires to (GPIO 23 and GPIO 24) nothing yet respectively, now I just need to test if they are working properly

#1:22 Okay I connected everything and there aren’t any errors in the code, but the servo motors aren’t moving, I’m not sure what’s wrong, maybe I need to change the GPIO pins in the code to match the ones I connected to, or the power bank is bad

#1:24 Well looks like the power bank is bad, I’m gonna try to find another

#2:17 I troubleshooted a lot and I think the problem is that the breadboard wiring is really bad. I ordered a usb power module so I can connect it directly to the breadboard and then connect the servos to that, hopefully that will fix the problem and I won’t have to deal with the messy wiring anymore

Devlog3_RoboSnoop_10/06/26
FINALLY aligned the second slider with the first and completed the code for the calculation of the angle for the index finger (unfortunately we have been lied to, A-level mathematics is not completely useless):

#Find vector AB
  const BA = {
    x: A.x - B.x,
    y: A.y - B.y
  };
#Find vector  CB
  const BC = {
    x: C.x - B.x,
    y: C.y - B.y
  };
#Calculate the dot product
  const dotProduct =
    BA.x * BC.x +
    BA.y * BC.y;
#mod AB = (x^2 + y^2)^0.5
  const magnitudeBA =
    Math.sqrt(BA.x * BA.x + BA.y * BA.y);

  const magnitudeBC =
    Math.sqrt(BC.x * BC.x + BC.y * BC.y);
#dot product = modAB * mod BC cos theta (manipulate to calculate theta)
  const angle = Math.acos(
    dotProduct /
    (magnitudeBA * magnitudeBC)
  );

  return angle * 180 / Math.PI;
}

hands.onResults((results) => {

  if (!results.multiHandLandmarks.length) return;

  const hand = results.multiHandLandmarks[0];
#5, 6, 7 represent the different joints in the finger
  const indexAngle =
    getAngle(hand[5], hand[6], hand[8]);

  setAngleValue(indexAngle);

The next steps would be adding 3 more sliders for each of the fingers (and aligning them so that they aren’t floating everywhere), and hopefully accounting for another degree of freedom about the knuckle joint (right now there’s only one degree I calculated for so only one value of theta, but there’s 3 degrees of freedom about the index finger (not accounting for adduction and abduction) and I have to figure out how to work around that or incorporate it into RoboSnoop and ultimately Project Handy

• Snoopsawg

Devlog2_RoboSnoop_10/06/26
I figured out how to attach a slider to the RoboSnoop OS, but I only got it to move based on the position of my hand along the x axis of the screen instead of based on the angle at which my fingers curl.

<script src="https://cdn.jsdelivr.net/npm/@mediapipe/control_utils/control_utils.js"></script>
<script src="https://cdn.jsdelivr.net/npm/@mediapipe/drawing_utils/drawing_utils.js"></script>
<script src="https://cdn.jsdelivr.net/npm/@mediapipe/hands/hands.js"></script>

<video id="camera" autoplay playsinline></video>
<canvas id="output"></canvas>

<script>
  const slider = document.querySelector("#slider");
  let thumbAngle = slider.value;

  slider.addEventListener("input", function () {
    thumbAngle = slider.value;
  });
</script>
<script> 
  const hands = new Hands({
    locateFile: (file) => {
      return `https://cdn.jsdelivr.net/npm/@mediapipe/hands/${file}`;
    },
  });
  hands.setOptions({
    maxNumHands: 1,
    modelComplexity: 1,
    minDetectionConfidence: 0.5,
    minTrackingConfidence: 0.5,
  });
  hands.onResults((results) => {console.log("Found the handdd!", results)});
  const camera = new Camera (video,{onFrame: async () => {
    await hands.send({image: video});
  }, width: 700, height: 500});
  camera.start();
  hands.onResults((results) => {

if (!results.multiHandLandmarks || !results.multiHandLandmarks.length) return;

const indexTip = results.multiHandLandmarks[0][8]; // index fingertip
const min = Number(slider.min);
const max = Number(slider.max);

const value = min + indexTip.x * (max - min);
slider.value = value;
thumbAngle = value;
});

</script>

Devlog1_RoboSnoop_OS 10/06/26
Today I finished the code to begin the OS: inserting a webcam, adding a filter and headings to the system. Later on I’ll have to update the hand-tracking system so that it can follow along with angles for each finger and update the UI with bars which fluctuate based on micro-movements.

  body {
    background-color: #000000;
    background-image: radial-gradient(circle at top, rgba(28, 32, 64, 0.35), rgba(0, 0, 0, 0.95) 60%);
    color: rgb(255, 255, 255);
    font-family: "Headfont", Arial, sans-serif;
    text-align: left;
    position: absolute;
    left: 20px;
    top: 2px;
  }

  #container {
    margin: 0px auto;
    width: 700px;
    height: 500px;
     border: 5px solid rgba(107, 132, 255, 0.9);
     box-shadow: 0 0 18px rgba(107, 132, 255, 0.78), 0 0 44px rgba(53, 77, 255, 0.42), inset 0 0 24px rgba(0, 0, 0, 0.75);
    position: relative;
  }

  #videoElement {
    width: 700px;
    height: 500px;
    background-color: #000000;
     filter: saturate(4.4) contrast(2.1) hue-rotate(250deg) brightness(1.03) grayscale(0.12) drop-shadow(0 0 10px rgba(105, 145, 255, 0.48));
  }

  #overlay {
    position: absolute;
    inset: 0;
    background:
      linear-gradient(rgba(0, 0, 0, 0.18) 50%, rgba(255, 255, 255, 0.04) 50%),
      linear-gradient(90deg, rgba(40, 50, 120, 0.10), rgba(70, 95, 235, 0.07), rgba(10, 10, 10, 0.16));
    background-size: 100% 6px, 100% 100%;
    mix-blend-mode: screen;
    opacity: 0.7;
    pointer-events: none;
  }

  #overlay::before {
    content: "";
    position: absolute;
    inset: 0;
    background:
      radial-gradient(circle at 20% 20%, rgba(95, 130, 255, 0.38), transparent 24%),
      radial-gradient(circle at 80% 30%, rgba(70, 105, 240, 0.30), transparent 22%),
      radial-gradient(circle at 50% 75%, rgba(190, 210, 255, 0.16), transparent 26%);
    mix-blend-mode: screen;
  }
</style>

<div id="container">
  <video autoplay playsinline id="videoElement"></video>
  <div id="overlay"></div>
</div>

<script>
  const video = document.querySelector("#videoElement");

  navigator.mediaDevices
    .getUserMedia({ video: true })
    .then(function (stream) {
      video.srcObject = stream;
    })
    .catch(function (error) {
      console.error("Whoopsie:", error);
    });
</script>

Devlog 10/06/26: Finished the basic code for the hand tracker and connected the first servo!
The basic code was fairly simple and just tracks the motion of my index finger for now, but I’m having trouble capturing the micromovements fairly smoothly. I also need to find an external power source to connect all 5+ servos; right now, the one is just running connected directly to the Pi. I also need to start planning the 3D frame for printing soon and figure out the mechanics.

• Snoopsawg