# Push matrix and trig functions help (robot BD-1)

I am trying to model a robot in processing that I will build in real life.

Here is a pic,
https://www.thingiverse.com/thing:3683124

I will have 3 points of inflection per leg, plus 3 more for the neck.
I would like to start by modeling a leg in processing that I can move with the mouse (Drag) and Serial out the 3 angles of inflexion (Hip , knee, ankle) to the Arduino running the rotary actuators .

Later on we could study where the center of gravity will fall and other things. I am overwhelmed with the project, the Arduino Motor control is complex . Trig was never my strongest thing,

If anyone would like to collaborate, it would be greatâ€¦

1 Like
2 Likes

Chisir

Thanks a lot, I started working it from that example,

I was able to generate and adjust the first segment (hip to knee) but I have a hard time keeping the Femur (segment length) constant,

I tried a condition in the Mouse dragged Fun, but did not work,

Can you help?

Thanks

float Femur = 143;
float Tibia = 180;
float Fascia = 115;
float x, y, x2, y2;
int dragX, dragY, moveX, moveY;

float OriginX=600;
float OriginY=400;
/// mouse over Rect section
float bx, by;
int bs = 5;
boolean bover = false;
boolean locked = false;
float bdifx = 0.0;
float bdify = 0.0;
/////////

int w= #FFFFFF; // white
int yl= #FCF442; //yellow
int g= #A0FFA3; //green
int b= #64E1FF; // blue
int m= #CC15D3; // Magenta
int o= #FF6C67;  // orange
int i= #B767FF; // indigo
int r= #FC0B03; // red
int bk = (#212121);

void setup() {
size(1790, 1100, JAVA2D);

/// rect
bx = width/2.0;
by = height/2.0;
/////

}

void draw() {
background(0);

fill(255);
if (mouseX > bx-bs && mouseX < bx+bs &&  mouseY > by-bs && mouseY < by+bs)
{ bover = true;
if(!locked) {
stroke(255);
fill(153);
}
}
else {
stroke(153);
fill(153);
bover = false;
}
rect(bx, by, bs, bs, 4);

strokeWeight(20.0);
stroke(255, 100);
line(OriginX, OriginY, bx, by);

float dx = OriginX - bx;
float dy = abs (OriginY - by);

float AngHip = atan(dx/dy);
Femur = sqrt (pow(dx, 2)+pow(dy,2));

println ("Dist X:"+dx + " Dist Y:" +dy);
println ("AngHip: " + degrees(AngHip),2);
println ("femur: " + Femur,2);
}

void mousePressed() {
if(bover) {
locked = true;
fill(r);
} else
{ locked = false;
}

bdifx = mouseX-bx;
bdify = mouseY-by;

}

void mouseDragged() {
if(locked) //  && Femur ==143)
{
bx = mouseX-bdifx;
by = mouseY-bdify;
}
}

void mouseReleased() {
locked = false;
}

1 Like

see â€śpass to Arduinoâ€ť to find your 4 angles

(new version)

// see pass to Arduino to find your 4 angles

// https://processing.org/examples/reach1.html

// constants for type of segment
final int none=0;     // = upper segment of leg
final int rightFoot=1; // lower segment right
final int leftFoot=2;  // lower segment left

//---------------
// for right leg

float xRight, yRight,
x2Right, y2Right;
boolean lockRight=false;
boolean firstTimeRight=true;

float dxRight, dyRight,
txRight=0, tyRight=0,
angle1Right=0;   // pass to Arduino
float angle2Right; // pass to Arduino

// ----------------
// for left leg

float xLeft, yLeft,
x2Left, y2Left;
boolean lockLeft=false;
boolean firstTimeLeft=true;

float dxLeft, dyLeft,
txLeft=0, tyLeft=0,
angle1Left=0;  // pass to Arduino
float angle2Left; // pass to Arduino

// ----------------
// other vars

float segLength = 80;    // len of 1 segment
PGraphics pg;            // invisible PGraphics (press any key to see it). It's used to check mouse against red and blue circle.

//--------------------------------------------------------------------------------------
// Core functions

void setup() {
size(640, 360);

xRight = width/2;
yRight = height/2;
x2Right = xRight;
y2Right = yRight;

xLeft = width/2+40;
yLeft = height/2-19;
x2Left = xLeft;
y2Left = yLeft;

pg = createGraphics(width, height);
}

void draw() {
background(209);

// decoration and texts
texts();

// main part
pg.beginDraw();
pg.background(0);
pg.stroke(255);

leftLeg();
rightLeg();

pg.endDraw();

// For testing pg
if (keyPressed) {
image(pg, 0, 0);
}
}
// --------------------------------------------------------------------------------------------------------------
// Inputs functions

void mousePressed() {
if (pg.get(mouseX, mouseY) == color(255, 0, 0) ) {
lockRight=true;
return;
}

if (pg.get(mouseX, mouseY) == color(0, 0, 255) ) {
lockLeft=true;
return;
}
}

void mouseReleased() {
lockRight=false;
lockLeft=false;
}

// --------------------------------------------------------------------------------------------------------------
// Leg functions and segment

void rightLeg() {

// the first time we use pre-defined values
if (firstTimeRight) {
dxRight=width/2-30;
dyRight=height;
angle1Right= atan2(dyRight, dxRight);
txRight = width/2-30 - cos(angle1Right) * segLength;
tyRight = height - sin(angle1Right) * segLength;
}

// when mouse is hold, we use its values
if (lockRight) {
dxRight = mouseX - xRight;
dyRight = mouseY - yRight;

angle1Right = atan2(dyRight, dxRight);

txRight = mouseX - cos(angle1Right) * segLength;
tyRight = mouseY - sin(angle1Right) * segLength;

firstTimeRight=false;
}

dxRight = txRight - x2Right;
dyRight = tyRight - y2Right;
angle2Right = atan2(dyRight, dxRight);
xRight = x2Right + cos(angle2Right) * segLength;
yRight = y2Right + sin(angle2Right) * segLength;

segment(xRight, yRight, angle1Right, rightFoot); // lower part of leg
segment(x2Right, y2Right, angle2Right, none);  // upper part of leg
}

void leftLeg() {

// the first time we use pre-defined values
if (firstTimeLeft) {
dxLeft=width/2-30;
dyLeft=height;
angle1Left= atan2(dyLeft, dxLeft);
txLeft = width/2-30 - cos(angle1Left) * segLength;
tyLeft = height - sin(angle1Left) * segLength;
}

// when mouse is hold, we use its values
if (lockLeft) {
dxLeft = mouseX - xLeft;
dyLeft = mouseY - yLeft;

angle1Left = atan2(dyLeft, dxLeft);
// angle1Left = constrain(angle1Left, 0, PI);    // ??????????

txLeft = mouseX - cos(angle1Left) * segLength;
tyLeft = mouseY - sin(angle1Left) * segLength;

firstTimeLeft=false;
}

dxLeft = txLeft - x2Left;
dyLeft = tyLeft - y2Left;
angle2Left = atan2(dyLeft, dxLeft);
xLeft = x2Left + cos(angle2Left) * segLength;
yLeft = y2Left + sin(angle2Left) * segLength;

segment(xLeft, yLeft, angle1Left, leftFoot); // lower part of leg
segment(x2Left, y2Left, angle2Left, none); // upper part of leg
}

void segment(float x, float y,
float a, // angle
int type) {

fill(0);

pushMatrix();
translate(x, y);
rotate(a);

strokeWeight(20.0);
stroke(255, 100);  // WHITE
line(0, 0, segLength, 0);

strokeWeight(5.0);
stroke(255, 0, 0, 100); //RED
line(10, 0, segLength-17, 0);

switch (type) {

case none:
break;

case rightFoot: // right lower leg
fill(0, 255, 0);
ellipse(segLength, 0,
6, 6);
if (lockRight)ellipse(segLength, 0,
16, 16);

pg.pushMatrix();
pg.translate(x, y);
pg.rotate(a);
pg.translate(segLength, 0);
pg.fill(255, 0, 0); //RED
pg.ellipse(0, 0, 25, 25);
pg.popMatrix();
break;

case leftFoot:
fill(0, 255, 0);
ellipse(segLength, 0,
6, 6);
if (lockLeft)ellipse(segLength, 0,
16, 16);

pg.pushMatrix();
pg.translate(x, y);
pg.rotate(a);
pg.translate(segLength, 0);
pg.fill( 0, 0, 255); // BLUE
pg.ellipse(0, 0, 25, 25);
pg.popMatrix();
break;
}//switch

popMatrix();
}

//-----------------------------------------------------------------------------------------------
// Misc functions

void texts() {

fill(0); // Black
text("Drag left or right foot to change angles (left and right as seen from the robot) ", width/2-133, 14);

text("Angles: \n"
+"L Upper " + angle2Left +"\n"  // upper part
+"L Lower " + angle1Left+"\n"   // lower part
+"R Upper " + angle2Right+"\n" // upper part
+"R Lower " + angle1Right+"\n", // lower part
13, 14);

if (lockRight)
text("Right leg", width/2+210, 127);
if (lockLeft)
text("Left leg", width/2+210, 127);
}

// antenna
antenna(width/2, height/2-30 );
antenna(width/2+23, height/2-45 );

fill(244);
//stroke(0);
noStroke();
rect(width/2, height/2-30,
60, 26);

fill(0);
ellipse (width/2+15, height/2-15,
18, 18);

stroke(255, 0, 0); // RED
line(width/2+3, height/2-27,
width/2+43, height/2-27);

line(width/2+43+8, height/2-27,
width/2+43+8-6, height/2-27+14);

//text
fill(0);
text("I look right",
width/2+111, height/2-2);
}//func

void antenna (int a, int b) {
strokeWeight(2.0);
stroke(88);
line (a, b,
a-30, b-50);
fill(0);
ellipse  (a-30, b-50, 6, 6);
}
//
1 Like

Wow, very elabrorate

Thank you a bunch, I will get back to you with questions

Mitch

Chisir,
This is exactly what I need, but my skill level is not there to perfect it.

So I wonder if youâ€™d want to work on it till we get what we need. I can afford a reasonable consulting fee, if youâ€™re interested.

To give you an idea, here is what we need:

Both legs should origin at the same hip point.
Lengths of the segments are different:
float Femur = 143;
float Tibia = 180;
float Fascia = 115;
float PelvisHeight = 105;

We would need to ad a Fascia (a Foot) to each leg,

Angles to generate, please one angle array per foot:
Hip Angle
Knee Angle
Fascia Angle

The Pelvis should have an drag point for leaning forward or back. This will affect the hip angle.

There should be an â€śEarthâ€ť where the fascia can go no lower.
The knee angles should be constrained so the bending is only towards the back,
The pelvis leg origin should be adjustable too, so there is a â€śhigh walkâ€ť and â€ślow walkâ€ť

We also need to generate a vertical center of gravity line to see how it falls on the legs position.

I would love to work with you, I can send some pics of what I mean but the forum has no uploadsâ€¦

Mitch
Haralambie@Yahoo.com

2 Likes

Unfortunately I donâ€™t have time for this

But itâ€™s a very nice project

You can upload images here by the way

Lower right corner - landscape symbol

Thanks, I understand youâ€™re busy,

Here is a pic of the mechanical leg,

I tried working off your sketch, but did not get far,
It is all based on â€śsegmentâ€ť being equal from thigh to shin.
If you could re-write it where I can define the length of each segment, that would be a great start for me.

Mitch

What do you mean by this? In x-direction or y direction or both?

Let me explain:

When both leg origins are in the same point, they would overlap.Bad.

At the moment, I draw the robot a bit like in 3D, when you look at the antenna, you see what I mean. In the same way, I draw the legs:

• the left leg (as seen from the robot) a little right and above the other.

Like in the image https://www.thingiverse.com/thing:3683124

Actually, yourâ€™re right, It would be harder to distinguish the legs. (I wanted to make them different colors)
Iâ€™m only exporting angles. Lets leave them like that,

is this the height of the pelvis (plus head) OR the height above earth / ground ?

new version, different segment lengths thigh / shin

gotta go

Chrisir

// see "pass to Arduino" to find your 6 angles
// https://processing.org/examples/reach1.html
// https://discourse.processing.org/t/push-matrix-and-trig-functions-help/16795/4

// -----------------------------------------------------------------------------------

// constants for different lengths
final float segLengthUpperPart = 143;    // len of 1 segment // uper leg / thigh / Femur
final float segLengthLowerPart = 180;    // len of 1 segment // lower leg / shin / Tibia
final float PelvisHeight = 105;          // height pelvis plus head

// constants for type of segment
final int none=0;     // = upper segment of leg
final int rightFoot=1; // lower segment right
final int leftFoot=2;  // lower segment left

//---------------
// for right leg

float xRight, yRight,
x2Right, y2Right;
boolean lockRight=false;
boolean firstTimeRight=true;

float dxRight, dyRight,
txRight=0, tyRight=0,
angle1Right=0;   // pass to Arduino
float angle2Right; // pass to Arduino
float angle3RightFoot; // pass to Arduino

// ----------------
// for left leg

float xLeft, yLeft,
x2Left, y2Left;
boolean lockLeft=false;
boolean firstTimeLeft=true;

float dxLeft, dyLeft,
txLeft=0, tyLeft=0,
angle1Left=0;  // pass to Arduino
float angle2Left; // pass to Arduino
float angle3LeftFoot; // pass to Arduino

// ----------------
// other vars
PGraphics pg;            // invisible PGraphics (press any key to see it). It's used to check mouse against red and blue circle.

//--------------------------------------------------------------------------------------
// Core functions

void setup() {
size(1140, 760);

xRight = width/2;
yRight = height/2;
x2Right = xRight;
y2Right = yRight;

xLeft = width/2+40;
yLeft = height/2-19;
x2Left = xLeft;
y2Left = yLeft;

pg = createGraphics(width, height);
}

void draw() {
background(209);

// decoration and texts
texts();

// main part
pg.beginDraw();
pg.background(0);
pg.stroke(255);

leftLeg();
rightLeg();

pg.endDraw();

// For testing pg
if (keyPressed) {
image(pg, 0, 0);
}
}
// --------------------------------------------------------------------------------------------------------------
// Inputs functions

void mousePressed() {
// mouse locked to foot? Via pg
if (pg.get(mouseX, mouseY) == color(255, 0, 0) ) {
lockRight=true;
return;
}

if (pg.get(mouseX, mouseY) == color(0, 0, 255) ) {
lockLeft=true;
return;
}
}

void mouseReleased() {
lockRight=false;
lockLeft=false;
}

// --------------------------------------------------------------------------------------------------------------
// Leg functions and segment

void rightLeg() {

// the first time we use pre-defined values
if (firstTimeRight) {
dxRight=width/2-30;
dyRight=height;
angle1Right= atan2(dyRight, dxRight);
txRight = width/2-30 - cos(angle1Right) * segLengthLowerPart;
tyRight = height - sin(angle1Right) * segLengthLowerPart;
}

// when mouse is hold, we use its values
if (lockRight) {
dxRight = mouseX - xRight;
dyRight = mouseY - yRight;

angle1Right = atan2(dyRight, dxRight);

txRight = mouseX - cos(angle1Right) * segLengthLowerPart;
tyRight = mouseY - sin(angle1Right) * segLengthLowerPart;

firstTimeRight=false;
}

dxRight = txRight - x2Right;
dyRight = tyRight - y2Right;
angle2Right = atan2(dyRight, dxRight);
xRight = x2Right + cos(angle2Right) * segLengthUpperPart;
yRight = y2Right + sin(angle2Right) * segLengthUpperPart;

segment(xRight, yRight, angle1Right, segLengthLowerPart, rightFoot);  // lower part of leg
segment(x2Right, y2Right, angle2Right, segLengthUpperPart, none);     // upper part of leg
}

void leftLeg() {

// the first time we use pre-defined values
if (firstTimeLeft) {
dxLeft=width/2-30;
dyLeft=height;
angle1Left= atan2(dyLeft, dxLeft);
txLeft = width/2-30 - cos(angle1Left) * segLengthLowerPart;
tyLeft = height - sin(angle1Left) * segLengthLowerPart;
}

// when mouse is hold, we use its values
if (lockLeft) {
dxLeft = mouseX - xLeft;
dyLeft = mouseY - yLeft;

angle1Left = atan2(dyLeft, dxLeft);
angle1Left = constrain(angle1Left, 0, PI);

txLeft = mouseX - cos(angle1Left) * segLengthLowerPart;
tyLeft = mouseY - sin(angle1Left) * segLengthLowerPart;

firstTimeLeft=false;
}

dxLeft = txLeft - x2Left;
dyLeft = tyLeft - y2Left;
angle2Left = atan2(dyLeft, dxLeft);
xLeft = x2Left + cos(angle2Left) * segLengthUpperPart;
yLeft = y2Left + sin(angle2Left) * segLengthUpperPart;

segment(xLeft, yLeft, angle1Left, segLengthLowerPart, leftFoot); // lower part of leg
segment(x2Left, y2Left, angle2Left, segLengthUpperPart, none); // upper part of leg
}

void segment(float x, float y,
float a, // angle
float segLength,
int type) {

fill(0);

pushMatrix();
translate(x, y);
rotate(a);

strokeWeight(20.0);
stroke(255, 100);  // WHITE
line(0, 0, segLength, 0);

strokeWeight(5.0);
stroke(255, 0, 0, 100); //RED
line(10, 0, segLength-17, 0);

switch (type) {

case none:
break;

case rightFoot: // right lower leg
fill(0, 255, 0); // GREEN
ellipse(segLength, 0,
6, 6);
if (lockRight)ellipse(segLength, 0,
16, 16);

// Foot ------
fill(255); // WHITE
noStroke();
translate(segLength, 0);
rotate(-a);
angle3RightFoot=-a;
rect(0, 0,
30, 10);

pg.pushMatrix();
pg.translate(x, y);
pg.rotate(a);
pg.translate(segLength, 0);
pg.fill(255, 0, 0); //RED
pg.ellipse(0, 0, 25, 25);
pg.popMatrix();
break;

case leftFoot:
fill(0, 255, 0);
ellipse(segLength, 0,
6, 6);
if (lockLeft)ellipse(segLength, 0,
16, 16);

// Foot ------
fill(255); // WHITE
noStroke();
translate(segLength, 0);
rotate(-a);
angle3LeftFoot=-a;
rect(0, 0,
30, 10);

pg.pushMatrix();
pg.translate(x, y);
pg.rotate(a);
pg.translate(segLength, 0);
pg.fill( 0, 0, 255); // BLUE
pg.ellipse(0, 0, 25, 25);
pg.popMatrix();
break;
}//switch

popMatrix();
}

//-----------------------------------------------------------------------------------------------
// Misc functions

void texts() {

fill(0); // Black
text("Drag left or right foot (green circles) to change angles (left and right as seen from the robot) ", width/2-133, 14);

text("Angles: \n"
+"L Upper " + angle2Left +"\n"  // upper part
+"L Lower " + angle1Left+"\n"   // lower part
+"L Foot " + angle3LeftFoot+"\n\n"   // foot part

+"R Upper " + angle2Right+"\n" // upper part
+"R Lower " + angle1Right+"\n" // lower part
+"R Foot " + angle3RightFoot+"\n", // foot part
13, 14);

if (lockRight)
text("Right leg", width/2+210, 127);
if (lockLeft)
text("Left leg", width/2+210, 127);
}

// antenna
antenna(width/2, height/2-30-80 );
antenna(width/2+23, height/2-45-80 );

fill(244);
//stroke(0);
noStroke();
rect(width/2, height/2-PelvisHeight-12,
60, PelvisHeight);

pushMatrix();
translate(0, -PelvisHeight+22);
fill(0);
ellipse (width/2+15, height/2-15,
18, 18);

stroke(255, 0, 0); // RED
line(width/2+3, height/2-27,
width/2+43, height/2-27);

line(width/2+43+8, height/2-27,
width/2+43+8-6, height/2-27+14);

popMatrix();

//text
fill(0);
text("I look right",
width/2+111, height/2-2);
}//func

void antenna (int a, int b) {
strokeWeight(2.0);
stroke(88);
line (a, b,
a-30, b-50);
fill(0);
ellipse  (a-30, b-50, 6, 6);
}
//

Thanks a lot, I tried it and it all makes sense.

I can run with it from here,

Awesome,

I will share the Nobel price with you one day,

Mitch

hey, I just implemented auto movement

you canâ€™t use the mouse anymore

// see "pass to Arduino" to find your 6 angles
// https://processing.org/examples/reach1.html
// https://discourse.processing.org/t/push-matrix-and-trig-functions-help/16795/4

// -----------------------------------------------------------------------------------

// constants for different lengths
final float segLengthUpperPart = 143;    // len of 1 segment // uper leg / thigh / Femur
final float segLengthLowerPart = 180;    // len of 1 segment // lower leg / shin / Tibia
final float PelvisHeight = 105;          // height pelvis plus head

// constants for type of segment
final int none=0;     // = upper segment of leg
final int rightFoot=1; // lower segment right
final int leftFoot=2;  // lower segment left

//---------------
// Auto movement
float mouseXL, mouseYL;
float mouseXR, mouseYR;
float angleAutoR=0, angleAutoL=PI;
//---------------
// for right leg

float xRight, yRight,
x2Right, y2Right;
boolean lockRight=true;
boolean firstTimeRight=true;

float dxRight, dyRight,
txRight=0, tyRight=0,
angle1Right=0;   // pass to Arduino
float angle2Right; // pass to Arduino
float angle3RightFoot; // pass to Arduino

// ----------------
// for left leg

float xLeft, yLeft,
x2Left, y2Left;
boolean lockLeft=true;
boolean firstTimeLeft=true;

float dxLeft, dyLeft,
txLeft=0, tyLeft=0,
angle1Left=0;  // pass to Arduino
float angle2Left; // pass to Arduino
float angle3LeftFoot; // pass to Arduino

// ----------------
// other vars
PGraphics pg;            // invisible PGraphics (press any key to see it). It's used to check mouse against red and blue circle.

//--------------------------------------------------------------------------------------
// Core functions

void setup() {
size(1100, 760);

xRight = width/2;
yRight = height/2;
x2Right = xRight;
y2Right = yRight;

xLeft = width/2+40;
yLeft = height/2-19;
x2Left = xLeft;
y2Left = yLeft;

pg = createGraphics(width, height);
}

void draw() {
background(209);

// decoration and texts
texts();

// main part
pg.beginDraw();
pg.background(0);
pg.stroke(255);

leftLeg();
rightLeg();

pg.endDraw();

// For testing pg
if (keyPressed) {
image(pg, 0, 0);
}

// Auto movement

angleAutoL += 0.1;
angleAutoR += 0.1;
}
// --------------------------------------------------------------------------------------------------------------
// Inputs functions

//void mousePressed() {
//  // mouse locked to foot? Via pg
//  if (pg.get(mouseX, mouseY) == color(255, 0, 0) ) {
//    lockRight=true; //Yes
//    return;
//  }

//  if (pg.get(mouseX, mouseY) == color(0, 0, 255) ) {
//    lockLeft=true; //Yes
//    return;
//  }
//}

//void mouseReleased() {
//  // unlock
//  lockRight = false;
//  lockLeft  = false;
//}

// --------------------------------------------------------------------------------------------------------------
// Leg functions and segment

void rightLeg() {

// the first time we use pre-defined values
if (firstTimeRight) {
dxRight=width/2-30;
dyRight=height;
angle1Right= atan2(dyRight, dxRight);
txRight = width/2-30 - cos(angle1Right) * segLengthLowerPart;
tyRight = height - sin(angle1Right) * segLengthLowerPart;
}

// when mouse is hold, we use its values
if (lockRight) {
dxRight = mouseXR - xRight;
dyRight = mouseYR - yRight;

angle1Right = atan2(dyRight, dxRight);

txRight = mouseXR - cos(angle1Right) * segLengthLowerPart;
tyRight = mouseYR - sin(angle1Right) * segLengthLowerPart;

firstTimeRight=false;
}

dxRight = txRight - x2Right;
dyRight = tyRight - y2Right;
angle2Right = atan2(dyRight, dxRight);
xRight = x2Right + cos(angle2Right) * segLengthUpperPart;
yRight = y2Right + sin(angle2Right) * segLengthUpperPart;

segment(xRight, yRight, angle1Right, segLengthLowerPart, rightFoot);  // lower part of leg
segment(x2Right, y2Right, angle2Right, segLengthUpperPart, none);     // upper part of leg
}

void leftLeg() {

// the first time we use pre-defined values
if (firstTimeLeft) {
dxLeft=width/2-30;
dyLeft=height;
angle1Left= atan2(dyLeft, dxLeft);
txLeft = width/2-30 - cos(angle1Left) * segLengthLowerPart;
tyLeft = height - sin(angle1Left) * segLengthLowerPart;
}

// when mouse is hold, we use its values
if (lockLeft) {
dxLeft = mouseXL - xLeft;
dyLeft = mouseYL - yLeft;

angle1Left = atan2(dyLeft, dxLeft);
angle1Left = constrain(angle1Left, 0, PI);

txLeft = mouseXL - cos(angle1Left) * segLengthLowerPart;
tyLeft = mouseYL - sin(angle1Left) * segLengthLowerPart;

firstTimeLeft=false;
}

dxLeft = txLeft - x2Left;
dyLeft = tyLeft - y2Left;
angle2Left = atan2(dyLeft, dxLeft);
xLeft = x2Left + cos(angle2Left) * segLengthUpperPart;
yLeft = y2Left + sin(angle2Left) * segLengthUpperPart;

segment(xLeft, yLeft, angle1Left, segLengthLowerPart, leftFoot); // lower part of leg
segment(x2Left, y2Left, angle2Left, segLengthUpperPart, none); // upper part of leg
}

void segment(float x, float y,
float a, // angle
float segLength,
int type) {

fill(0);

pushMatrix();
translate(x, y);
rotate(a);

strokeWeight(20.0);
stroke(255, 100);  // WHITE
line(0, 0, segLength, 0);

strokeWeight(5.0);
stroke(255, 0, 0, 100); //RED
line(10, 0, segLength-17, 0);

switch (type) {

case none:
break;

case rightFoot: // right lower leg
fill(0, 255, 0); // GREEN
ellipse(segLength, 0,
6, 6);
if (lockRight)ellipse(segLength, 0,
16, 16);

// Foot ------
fill(255); // WHITE
noStroke();
translate(segLength, 0);
rotate(-a);
angle3RightFoot=-a;
rect(0, 0,
30, 10);

pg.pushMatrix();
pg.translate(x, y);
pg.rotate(a);
pg.translate(segLength, 0);
pg.fill(255, 0, 0); //RED
pg.ellipse(0, 0, 25, 25);
pg.popMatrix();
break;

case leftFoot:
fill(0, 255, 0);
ellipse(segLength, 0,
6, 6);
if (lockLeft)ellipse(segLength, 0,
16, 16);

// Foot ------
fill(255); // WHITE
noStroke();
translate(segLength, 0);
rotate(-a);
angle3LeftFoot=-a;
rect(0, 0,
30, 10);

pg.pushMatrix();
pg.translate(x, y);
pg.rotate(a);
pg.translate(segLength, 0);
pg.fill( 0, 0, 255); // BLUE
pg.ellipse(0, 0, 25, 25);
pg.popMatrix();
break;
}//switch

popMatrix();
}

//-----------------------------------------------------------------------------------------------
// Misc functions

void texts() {

fill(0); // Black
text("Auto version (left and right as seen from the robot) ", width/2-133, 14);

text("Angles: \n"
+"L Upper " + angle2Left +"\n"  // upper part
+"L Lower " + angle1Left+"\n"   // lower part
+"L Foot " + angle3LeftFoot+"\n\n"   // foot part

+"R Upper " + angle2Right+"\n" // upper part
+"R Lower " + angle1Right+"\n" // lower part
+"R Foot " + angle3RightFoot+"\n", // foot part
13, 14);

text("AUTO Version", width/2+210, 127);
}

// antenna
antenna(width/2, height/2-30-80 );
antenna(width/2+23, height/2-45-80 );

fill(244);
//stroke(0);
noStroke();
rect(width/2, height/2-PelvisHeight-12,
60, PelvisHeight);

pushMatrix();
translate(0, -PelvisHeight+22);
fill(0);
ellipse (width/2+15, height/2-15,
18, 18);

stroke(255, 0, 0); // RED
line(width/2+3, height/2-27,
width/2+43, height/2-27);

line(width/2+43+8, height/2-27,
width/2+43+8-6, height/2-27+14);

popMatrix();

//text
fill(0);
text("I look right",
width/2+111, height/2-2);
}//func

void antenna (int a, int b) {
strokeWeight(2.0);
stroke(88);
line (a, b,
a-30, b-50);
fill(0);
ellipse  (a-30, b-50, 6, 6);
}
//

new version:

still automatic movement

but instead of a complete ellipsoid movement, the foot hits the ground now (and stays on the ground)

(so that the upper part of the ellipse is still an ellipse, but the lower part of the ellipse is now a line on the floor)

// version: auto version with floor

// see "pass to Arduino" to find your 6 angles
// https://processing.org/examples/reach1.html
// https://discourse.processing.org/t/push-matrix-and-trig-functions-help/16795/4

// -----------------------------------------------------------------------------------
// (left and right as seen from the robot)

// constants for different lengths
final float segLengthUpperPart = 143;    // len of 1 segment // uper leg / thigh / Femur
final float segLengthLowerPart = 180;    // len of 1 segment // lower leg / shin / Tibia
final float PelvisHeight       = 105;    // height pelvis plus head

// constants for type of segment
final int none      = 0;     // = upper segment of leg
final int rightFoot = 1;     // lower segment right (left and right as seen from the robot)
final int leftFoot  = 2;     // lower segment left (left and right as seen from the robot)

// other consts
int floorY = 0;
int offsetXLeftSidePerspective = 40; // offset for the left side (seen from the robot) x
int offsetYLeftSidePerspective = 19; // offset for the left side (seen from the robot) y

//---------------
// Auto movement
float mouseXL, mouseYL;
float mouseXR, mouseYR;
float angleAutoR=0, angleAutoL=PI;

//---------------
// for right leg

float xRight, yRight,
x2Right, y2Right;
boolean lockRight=true;
boolean firstTimeRight=true;

float dxRight, dyRight,
txRight=0, tyRight=0,
angle1Right=0;   // pass to Arduino
float angle2Right; // pass to Arduino
float angle3RightFoot; // pass to Arduino

// ----------------
// for left leg

float xLeft, yLeft,
x2Left, y2Left;
boolean lockLeft=true;
boolean firstTimeLeft=true;

float dxLeft, dyLeft,
txLeft=0, tyLeft=0,
angle1Left=0;  // pass to Arduino
float angle2Left; // pass to Arduino
float angle3LeftFoot; // pass to Arduino

// ----------------
// other vars
// PGraphics pg;         // invisible PGraphics (press any key to see it). It's used to check mouse against red and blue circle.

//--------------------------------------------------------------------------------------
// Core functions

void setup() {
size(1100, 760);

xRight = width/2;
yRight = height/2;
x2Right = xRight;
y2Right = yRight;

xLeft = width/2+offsetXLeftSidePerspective;
yLeft = height/2-offsetYLeftSidePerspective;
x2Left = xLeft;
y2Left = yLeft;

floorY=height-190;

//  pg = createGraphics(width, height);
}

void draw() {
background(209);

// decoration and texts
texts();

// main part
//pg.beginDraw();
//pg.background(0);
//pg.stroke(255);

leftLeg();
rightLeg();

//  pg.endDraw();

// For testing pg
//if (keyPressed) {
//  image(pg, 0, 0);
//}

// Auto movement
if (mouseYL > floorY-offsetYLeftSidePerspective-8)
mouseYL=floorY-offsetYLeftSidePerspective-8;

if (mouseYR > floorY-8)
mouseYR=floorY-8;

angleAutoL += 0.1;
angleAutoR += 0.1;

stroke(0);
strokeWeight(2);
line(0, floorY,
width, floorY);
}//func draw

// --------------------------------------------------------------------------------------------------------------
// Inputs functions

//void mousePressed() {
//  // mouse locked to foot? Via pg
//  if (pg.get(mouseX, mouseY) == color(255, 0, 0) ) {
//    lockRight=true; //Yes
//    return;
//  }

//  if (pg.get(mouseX, mouseY) == color(0, 0, 255) ) {
//    lockLeft=true; //Yes
//    return;
//  }
//}

//void mouseReleased() {
//  // unlock
//  lockRight = false;
//  lockLeft  = false;
//}

// --------------------------------------------------------------------------------------------------------------
// Leg functions and segment

void rightLeg() {

// the first time we use pre-defined values
if (firstTimeRight) {
dxRight=width/2-30;
dyRight=height;
angle1Right= atan2(dyRight, dxRight);
txRight = width/2-30 - cos(angle1Right) * segLengthLowerPart;
tyRight = height - sin(angle1Right) * segLengthLowerPart;
}

// when mouse is hold, we use its values
if (lockRight) {
dxRight = mouseXR - xRight;
dyRight = mouseYR - yRight;

angle1Right = atan2(dyRight, dxRight);

txRight = mouseXR - cos(angle1Right) * segLengthLowerPart;
tyRight = mouseYR - sin(angle1Right) * segLengthLowerPart;

firstTimeRight=false;
}

dxRight = txRight - x2Right;
dyRight = tyRight - y2Right;
angle2Right = atan2(dyRight, dxRight);
xRight = x2Right + cos(angle2Right) * segLengthUpperPart;
yRight = y2Right + sin(angle2Right) * segLengthUpperPart;

segment(xRight, yRight, angle1Right, segLengthLowerPart, rightFoot);  // lower part of leg
segment(x2Right, y2Right, angle2Right, segLengthUpperPart, none);     // upper part of leg
}

void leftLeg() {

// the first time we use pre-defined values
if (firstTimeLeft) {
dxLeft=width/2-30;
dyLeft=height;
angle1Left= atan2(dyLeft, dxLeft);
txLeft = width/2-30 - cos(angle1Left) * segLengthLowerPart;
tyLeft = height - sin(angle1Left) * segLengthLowerPart;
}

// when mouse is hold, we use its values
if (lockLeft) {
dxLeft = mouseXL - xLeft;
dyLeft = mouseYL - yLeft;

angle1Left = atan2(dyLeft, dxLeft);
angle1Left = constrain(angle1Left, 0, PI);

txLeft = mouseXL - cos(angle1Left) * segLengthLowerPart;
tyLeft = mouseYL - sin(angle1Left) * segLengthLowerPart;

firstTimeLeft=false;
}

dxLeft = txLeft - x2Left;
dyLeft = tyLeft - y2Left;
angle2Left = atan2(dyLeft, dxLeft);
xLeft = x2Left + cos(angle2Left) * segLengthUpperPart;
yLeft = y2Left + sin(angle2Left) * segLengthUpperPart;

segment(xLeft, yLeft, angle1Left, segLengthLowerPart, leftFoot); // lower part of leg
segment(x2Left, y2Left, angle2Left, segLengthUpperPart, none); // upper part of leg
}

void segment(float x, float y,
float a, // angle
float segLength,
int type) {

fill(0);

pushMatrix();
translate(x, y);
rotate(a);

strokeWeight(20.0);
stroke(255, 100);  // WHITE
line(0, 0, segLength, 0);

strokeWeight(5.0);
stroke(255, 0, 0, 100); //RED
line(10, 0, segLength-17, 0);

switch (type) {

case none:
break;

case rightFoot: // right lower leg
fill(0, 255, 0); // GREEN
ellipse(segLength, 0,
6, 6);
if (lockRight)ellipse(segLength, 0,
16, 16);

// Foot ------
fill(255); // WHITE
noStroke();
translate(segLength, 0);
rotate(-a);
angle3RightFoot=-a;
rect(0, 0,
30, 10);

//pg.pushMatrix();
//pg.translate(x, y);
//pg.rotate(a);
//pg.translate(segLength, 0);
//pg.fill(255, 0, 0); //RED
//pg.ellipse(0, 0, 25, 25);
//pg.popMatrix();
break;

case leftFoot:
fill(0, 255, 0);
ellipse(segLength, 0,
6, 6);
if (lockLeft)ellipse(segLength, 0,
16, 16);

// Foot ------
fill(255); // WHITE
noStroke();
translate(segLength, 0);
rotate(-a);
angle3LeftFoot=-a;
rect(0, 0,
30, 10);

//pg.pushMatrix();
//pg.translate(x, y);
//pg.rotate(a);
//pg.translate(segLength, 0);
//pg.fill( 0, 0, 255); // BLUE
//pg.ellipse(0, 0, 25, 25);
//pg.popMatrix();
break;
}//switch

popMatrix();
}

//-----------------------------------------------------------------------------------------------
// Misc functions

void texts() {

fill(0); // Black
text("Automatic version (left and right as seen from the robot) ", width/2-133, 14);

text("Angles: \n"
+"L Upper " + angle2Left +"\n"  // upper part
+"L Lower " + angle1Left+"\n"   // lower part
+"L Foot " + angle3LeftFoot+"\n\n"   // foot part

+"R Upper " + angle2Right+"\n" // upper part
+"R Lower " + angle1Right+"\n" // lower part
+"R Foot " + angle3RightFoot+"\n", // foot part
13, 14);

text("AUTO Version", width/2+210, 127);
}

// antenna
antenna(width/2, height/2-30-80 );
antenna(width/2+offsetXLeftSidePerspective, height/2-30-80-offsetYLeftSidePerspective );

fill(244);
//stroke(0);
noStroke();
rect(width/2, height/2-PelvisHeight-12,
60, PelvisHeight);

pushMatrix();
translate(0, -PelvisHeight+22);
fill(0);
ellipse (width/2+15, height/2-15,
18, 18);

stroke(255, 0, 0); // RED
line(width/2+3, height/2-27,
width/2+43, height/2-27);

line(width/2+43+8, height/2-27,
width/2+43+8-6, height/2-27+14);

popMatrix();

//text
fill(0);
text("I look right",
width/2+111, height/2-2);
}//func

void antenna (int a, int b) {
strokeWeight(2.0);
stroke(88);
line (a, b,
a-30, b-50);
fill(0);
ellipse  (a-30, b-50, 6, 6);
}
//

Wow that looks amazing.

I would not know what to do with it, yet. I was thinking to just pass 3-4 waypoints to Arduino and start experimenting.
If you want to help more, here is the drawing of the robot with dimensions.

If you could model the pelvis, neck and head it would be great. Maybe later we can find a center of gravity.

So it would be nice to have these points of inflection with drag and drop mouse capability:

• Pelvis attitude _( not always vertical)
-neck low motor
• neck high motor

I adjusted the legs origin point to be the same otherwise the angles to ardu are erroneous. Lets stay with the left side 2 d view for now. We can do a frontal view later.

It can also be done in 3 d, but thats lots of work.

I will have a â€śSend anglesâ€ť button after I adjust all points of inflection in Processing.(static)

It is nice you made a ground. The ground being adjustable (Up and Down) could help with making a â€śhigh walkâ€ť or lowerâ€¦ etc. Tilting the ground would be nice also, and generating all angles for uphill or downhillâ€¦

youâ€™re, great
Thanks a bunch

1 Like

You are welcome!

I donâ€™t have time to look into this but feel free to work from my code

Chrisir

Processing can display this

Maybe you can rotate the joints separately

1 Like

Yeah, Chrisir
Thanks for the suggestion, I will probably make brand new metal parts, Water jet cut, etc

I made great progress, I can move limbs from processing now, Thanks for the start

Mitch

2 Likes