Trying to find an equation for distance you can go based on Xpos and Ypos of a robot, robot angle, and distance from an object.

Question

So I have a robot, and I am trying to push an object into a red border without falling off the edge. the border is 100mm thick, the whitespace in between the 4 borders in the square is 2000x2000mm, and the total dimensions of the board are 2200x2200. I am trying to find an equation I can use for the distance I can go to push the block into the red border without falling off the edge, using the robot's X position, Y position, angle, and the distance from the object.

Answer 1

Your question is not a difficult proposition by any means. If you have drawn a decent diagram of the scenario described in your problem statement, probably you could have solved it yourself. It requires only fundamental knowledge of trigonometry and algebra, and Pythagoras’ theorem. Please note that all the lengths are given in $\pmb{meters}$.

As shown in our two sketches, $T_0$ and $T_1$ are the starting and the final position of the object pushed by the robot respectively. It is given that the point $T_0$ is located a distance of $a$ from the upper-right corner $E$ of the inner square. The starting position of the robot is $R_0$, while the origin $O$ of the coordinate system is at the bottom-left corner of the inner square. The values of $\space{\large{x}}_{R_0},\space {\large{y}}_{R_0},\space a,\space$ and $\space \theta\space$ are known and you are seeking an expression for $T_0T_1 \left(=b\right)$ in terms those four values.

There are two different scenarios depending on the values of $\theta$ and $\varphi$, where the latter is the angle between $T_0E$ and the $y\text{-axis}$. First of all, you need to determine $d$, the distance between the robot and the object at the start as shown below.

$$T_0C = OG – OA – R_0B=2-{\large{x}}_{R_0}-d\sin\left(\theta\right)\quad$$ $$EC=AF-AR_0-BT_0=2-{\large{y}}_{R_0}- d\cos\left(\theta\right)$$

Now, we apply Pythagoras’ theorem to the right-angled triangle $T_0CE$ to obtain, $$a^2=\left(2-{\large{x}}_{R_0}-d\sin\left(\theta\right)\right)^2+\left(2-{\large{y}}_{R_0}- d\cos\left(\theta\right)\right)^2.$$

After simplifying this, we rearrange its like terms together to produce the following quadratic equation in $d$. $$d^2{\small{-}}2\Big(\big(2{\small{-}}{\large{x}}_{R_0}\big)\sin\left(\theta\right){\small{+}} \big(2{\small{-}}{\large{y}}_{R_0}\big)\cos\left(\theta\right)\Big)d {\small{+}} 8{\small{-}}a^2{\small{+}} {\large{x}}_{R_0}^2 {\small{+}} {\large{y}}_{R_0}^2 {\small{-}} {\large{x}}_{R_0}{\small{-}} {\large{y}}_{R_0}=0 \tag{1}$$

Equation (1) has two positive real roots. It can be shown that the smaller of the two roots is the value of $d$. When we have $d$, we can find $\phi$ using properties of the right-angle triangle $T_0CE$. $$\varphi = \sin^{-1}\left(\dfrac{2-{\large{x}}_{R_0}-d\sin\left(\theta\right)}{a}\right). \tag{2}$$

Once we know the value of $\varphi$, we can use it to differentiate between the two scenarios.

In the first of the two possible scenarios (see $\mathrm{Fig.\space 1}$), we have $\phi \ge \theta$. For this case, following equation expressing the sought distance $b$ can be derived. using properties of the right-angle triangle $T_0DT_1$. $$b=\left(\dfrac{2.1-{\large{y}}_{R_0}}{\cos\left(\theta\right)}\right)-d\tag{3.1}$$

$\mathrm{Fig.\space 2}$ depicts the second scenario, which is characterized by the fact $\phi \le \theta$. In this instance, the equation for determining $b$ takes the form, $$b=\left(\dfrac{2.1-{\large{x}}_{R_0}}{\sin\left(\theta\right)}\right)-d.\tag{3.2}$$