mirror of
https://github.com/nasa/trick.git
synced 2024-12-19 21:27:54 +00:00
Sim robot (#1498)
* First commit of SIM_robot. * Updates to kinematic arm with controller * Working well enough, still needs some user interface besides trick-TV * Add end-effector path trace to graphics client. * Singularity control bug, remove printouts, udpate makefile * Improve SIM_robot variable server client. * Tidy up RobotDisplay.java * Tidying up * Removing warnings * Working on documentation * Updating documentation * Updating docs * Adding figures for documentation * Removing some stuff in the README carried over from the template * Tidying up * Position vector finally done * Updating based on feedback * Forward position kinematics completed with notation changes * First pass documentation done? * remove printout * Fix typos in text and filenames * Update README.md * Update README.md * made it smaller --------- Co-authored-by: John M. Penn <john.m.penn@nasa.gov>
This commit is contained in:
Sean Harmeyer
GitHub
parent
33823f1a0b
commit
fd8252e2c5
9
trick_sims/SIM_robot/Modified_data/realtime.py
Normal file
9
trick_sims/SIM_robot/Modified_data/realtime.py
Normal file
@ -0,0 +1,9 @@
|
||||
trick.real_time_enable()
|
||||
trick.exec_set_software_frame(0.050)
|
||||
trick.itimer_enable()
|
||||
|
||||
trick.exec_set_enable_freeze(True)
|
||||
trick.exec_set_freeze_command(True)
|
||||
|
||||
simControlPanel = trick.SimControlPanel()
|
||||
trick.add_external_application(simControlPanel)
|
||||
271
trick_sims/SIM_robot/README.md
Normal file
271
trick_sims/SIM_robot/README.md
Normal file
@ -0,0 +1,271 @@
|
||||
# Planar Robotic Manipulator Simulation
|
||||
|
||||
SIM_robot is a 2-dimensional planar robotic simulation of a 2 degree of freedom manipulator. It is a kinematic simulation, which means that it concerns itself solely with the position and velocity of the links but does not model or account for forces and torques. This helps keep the simulation more simple and easier to comprehend, while also providing the necessary framework to serve as an adequate example of the general layout of a robotics simulation. The controller described below and implemented in the simulation is both simple and extremely common. It is used extensively in both kinematic and dynamic robotic modeling and control.
|
||||
|
||||
The simulation calculates both forward and inverse kinematics and implements some common control schemes to allow the user to control the manipulator in a variety of ways. Only the tip (or end-effector) position of the manipulator is controlled, not the orientation of the last link.
|
||||
|
||||
Included is a graphics and user interface client which allows the user to view the motion of the robotic arm in the sim and to send moding and control inputs to the manipulator in real time. A circle indicates the maximum extent of the workspace of the manipulator.
|
||||
|
||||
|
||||
## Building the Simulation
|
||||
In order to build a Trick simulation, the Trick-specific command **```trick-CP```** must be run to begin compilation process. **```trick-CP```** is located in the Trick ```bin``` directory. Run the **```trick-CP```** command in the SIM_robot directory. Upon successful compilation, the following message will be displayed in the terminal:
|
||||
|
||||
```
|
||||
=== Simulation make complete ===
|
||||
```
|
||||
|
||||
The assocated graphics/user interface client should also be built automatically.
|
||||
|
||||
## Running the Simulation
|
||||
In the SIM_robot directory:
|
||||
|
||||
```
|
||||
% S_main_*.exe RUN_test/input.py
|
||||
```
|
||||
The Sim Control Panel, and a graphics client called "Robot Range" should appear.
|
||||
|
||||
The sim will come up in Freeze mode. Click Start on the Trick Sim Control Panel and the simulation time should start advancing.
|
||||
|
||||
|
||||
|
||||
|
||||
## User Control Options
|
||||
|
||||
There are three control modes available for this manipulator: Single, Manual, and EEPos. Each will be described below. Furthermore, the gui provides a toggle to enable tracing the end-effector (or tip) position.
|
||||
|
||||
### Single Joint Mode
|
||||
**Single Joint Mode** is the most straightfoward control mode in the simulation. It simply commands a selected joint to move with the angular velocity commanded. The user interface provides a button to select Single as your mode, a Joint select button to choose the desired joint, and a slider to command a desired joint velocity.
|
||||
|
||||
### Manual Mode
|
||||
**Manual Mode** enables control of the motion of the end-effector with a desired velocity. This mode commands the end-effector to move in the desired direction at the desired rate until either the velocity command is changed or the arm reaches a singularity (described below). The gui has an "EE Velocity" interface for interacting in this mode. The gray circle is like a dial, where the angle and location of the selected point in the circle determine the commanded direction and rate of the end-effector. For example, clicking directly underneath the center of the circle on the edge will command the end-effector to move straight down as fast as the mode allows. This input can be changed while moving, allowing the user to maneuver the end-effector however they see fit in real time.
|
||||
|
||||
### End-Effector Position Mode
|
||||
**End-Effector Position Mode** commands the end-effector to autonomously move to the selected point in the workspace. The user clicks anywhere on the gui display and the end-effector will attempt to move to that location. Clicking elsewhere on the gui will change the selected position and the manipulator will begin moving towards the new one. Singularities and reach limits may prevent it from reaching the point, however. The manipulator makes no effort to avoid these as a lesson in owning the consequences of one's actions.
|
||||
|
||||
## Kinematics of the System
|
||||
The kinematics of a robotic manipulator describe both the position and velocity of the manipulator at any point on the robot. Kinematics do not include accelerations, forces, or moments in their description.
|
||||
|
||||
In this sim, we will discuss both forward and inverse kinematics. Forward kinematics give the position/velocity of any location of interest on the manipulator given a set of joint angles/velocities. Inverse kinematics go in the reverse: Given a desired location/velocity for some point of interest on the manipulator, the equations supply the necessary joint angles/velocities.
|
||||
|
||||
The position of the end-effector is highly non-linear and heavily coupled with respect to the joint angles required to produce said position. However, the velocity of the end-effector is linearly related to the joint velocities required to produce it, which forms the basis of the controller described below. The non-linearity of the position equations, and the linearity of the velocity equations, will be shown in the next sections.
|
||||
|
||||
For a robotic system, the number of directions the end-effector can move is determined by the number of degrees of freedom, i.e. the number of joints, and how they are layed out. In general, each joint allows motion in another direction. This two degree-of-freedom manipulator can move in $x$ and $y$, but not $Z$. Adding more degrees of freedom than you need to move in all your desired directions makes a system *kinematically redundant*, and while those are super fun and interesting, they are beyond the scope of this tutorial.
|
||||
|
||||
|
||||
### How to Layout Points, Frames, and Joint Angles
|
||||
The position of the end-effector relative to some fixed Base frame location can be calculated by knowing the joint angles of the manipulator and the lengths of the manipulator links. First we will assign some points of interest along the manipulator that need to be kept track of during the forward kinematics calcualtions. We will define the point around which the first link's rotation is centered as point $a$, the point of the second link's rotation $b$, and the tip of the manipulator (our end-effector) as $e$. The distance between $a$ and $b$ will be defined as $L1$ and between $b$ and $e$ as $L2$.
|
||||
|
||||
|
||||
|
||||
But points of interest are of no use without frames of reference, so we need to define some frames in which we can define the relative locations of these points. Let's define a fixed, static Base (or Origin) frame O centered on point $a$. This frame will give us a reference frame to keep track of the location of points $a$, $b$, and $e$. Point $a$ is fixed, point $b$ moves when the first joint angle changes, and point $e$ moves when either joint angle changes. We need to keep track of how the links change their orientation with respect to both the base frame O and to each other. To do this we define reference coordinate frames to each link that move along with it. The convention for this sim is to point the X-axis of each frame along the length of these straight links, so that the X-axis points from each point of interest to the next ($a$ to $b$, $b$ to $e$). Frame O is fixed, allowing a constant reference frame to refer back to at any time.
|
||||
|
||||
|
||||
|
||||
Note: Frames are not fixed to one point, and can be moved around translationally wherever is convenient to help you visualize frame rotations. However, their orientations with respect to other frames is *not* arbitrary, and in this example are defined by the joint angles $q$. Sometimes it is convenient to place two frames on top of each other and co-located their origins (as seen in the figures above described generic frame rotations). Sometimes it is convenient to place the frames for each link on the tip of the link, to help visual the relative rotation between the previous link and the next one. You get to decide! Put them anywhere and everywhere, they're free of charge.
|
||||
|
||||
|
||||
We also need to keep track of the joint angles, since they can and will change during the run (otherwise it's less a manipulator and more a sculpture). Let's define the joint angles as $q1$ and $q2$, making 0 rotation align all three frames so that they are all oriented the same way. This way any rotation of a link can be easily described with joint angles $q1$ and $q2$.
|
||||
|
||||
|
||||
|
||||
|
||||
Now everything of importance to the kinematics of the system has been named and labeled. The base and joint frames allow us to keep track of the relative location of the points on the manipulator with respect to each other and the base, fixed inertial reference frame. The joint angles allow us to describe every possible location of each of the points in any frame.
|
||||
|
||||
### Frame Rotations
|
||||
Given that a robot will have *at least* as many frames as it has moveable links, we need to be able to transform the information about the manipulator from one frame to another. This is called frame rotation. One can construct a *rotation matrix* if the relative orientation of any two frames is known with respect to each other. In this example, we will be rotating between frames O, A, and B (specifically, from A and B back to O to express everything in O).
|
||||
|
||||
In general, if you have two frames, A and B, rotated with respect to each other by an angle $q$, then you can determine how the unit vectors which make up frame B relate to frame A:
|
||||
|
||||
|
||||
|
||||
Unit vectors (such as $\hat{X}_A$ and $\hat{Y}_A$) are by definition length 1. The cosine of the angle $q$ is defined as the length of the adjacent side divided by the hypotenuse. Since the length of the hypotenuse has to be 1, $cos(q)$ is equal to the length of the adjacent side. For $sin(q)$, you use the opposite side instead of the adjacent.
|
||||
|
||||
|
||||
|
||||
Given the information above, $X_B$ and $Y_B$ can be described with respect to frame A as:
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
These equations together can be written in matrix form as
|
||||
|
||||
|
||||
|
||||
This matrix is called a rotation matrix, from Frame A to Frame B, commonly denoted as
|
||||
|
||||
|
||||
|
||||
|
||||
This is the rotation matrix from Frame A to Frame B. The subscript frame (here, A) is the "from" frame and the superscript frame (here, B) is the "to" frame. If you take any vector described in frame A and pre-multiply it by this rotation matrix, you will get the same vector but expressed in frame B. More often than not, you will actually be more concerned with rotation from Frame B back to Frame A. To do this, you would need the inverse of the matrix (which reverses the frame transformation order). Luckily, rotations matrices are *orthonormal*, and one characteristic of matrices like this is that the inverse of this matrix is equal to the transpose (which is much easier to find in general).
|
||||
|
||||
|
||||
|
||||
This is the bread and butter of kinematics, so you will definitely see this again.
|
||||
|
||||
|
||||
### Position of the End-Effector
|
||||
The position of the tip of manipulator can be described by calculating the vectors from points $a$ to $b$ and from $b$ to $e$ and adding them together. Vectors can be added together if they are described in the same frame. Vectors are simply lengths in a particular direction.
|
||||
|
||||
The vector from points $a$ to $b$ is a function of the joint angle $q1$ and the length of the link $L1$ (described in the base frame O). The vector from points $b$ to $e$ is a function of the joint angle $q2$ and the length of the link $L2$ (described in the first link's frame A).
|
||||
|
||||
|
||||
|
||||
As can be seen in the figure above, the $x$ and $y$ components of the vector which goes from point $a$ to point $b$ is shown to be
|
||||
|
||||
|
||||
|
||||
and
|
||||
|
||||
|
||||
|
||||
expressed in the base frame O.
|
||||
|
||||
The $x$ and $y$ components of the vector from $b$ to $e$ is shown to be
|
||||
|
||||
|
||||
|
||||
and
|
||||
|
||||
|
||||
|
||||
expressed in the first link's frame, A.
|
||||
|
||||
In frame A, the vector from $a$ to $b$ is simply
|
||||
|
||||
|
||||
|
||||
because the axes of frame A rotate with the first link, so the vector always points along the $x$-axis of frame A. Similarly, the vector from $b$ to $e$ in frame B is
|
||||
|
||||
|
||||
|
||||
If we add the vectors from $a$ to $b$ to $e$ together, we'll get a vector from $a$ to $e$. This will tell us the location of the end-effector, which is what we're really after:
|
||||
|
||||
|
||||
|
||||
However they can't be added until they are all expressed in the same frame. To do that, we will rotate $P_{ab}$ and $P_{be}$ into frame O, the static base frame, and then add them together. In order to do that, we will use rotation matrices as discussed in the previous section.
|
||||
|
||||
First, rotate the vector $P_{OB}$ into frame O by use of the rotation matrix from frame A into frame O (remembering that $R_A^B = R{_B^A}^T$)
|
||||
|
||||
|
||||
|
||||
This matches what was shown earlier, that the vector $P_{ab}$ in frame O had components that match what was just derived using rotation matrices.
|
||||
|
||||
For the vector $P_{BE}$, two transformations are needed. First, from frame B back to A, then from A back to O. Rotation matrices can be prepended as long as the origin and destination frames line up logically. In this case, a vector in frame B is being rotated to frame A, and that resultant vector is then rotated to frame O:
|
||||
|
||||
|
||||
|
||||
which leads to
|
||||
|
||||
|
||||
|
||||
which finally gives
|
||||
|
||||
.
|
||||
|
||||
Now the vectors $P_{ab}$ and $P_{be}$ can be added together, since they are both expressed in frame O
|
||||
|
||||
|
||||
|
||||
which, after some trigonometric wizardry, boils down to
|
||||
|
||||
|
||||
|
||||
|
||||
### Velocity of the End-Effector
|
||||
|
||||
Now that the position of all the points of interest are calculated, we need to find the velocities as well. For this example the only point of interest for which we will calculate the velocity is for point $e$, the tip of the manipulator. In general, for more complex robotic systems, the velocity of each point is calculated through a process known as *velocity propagation*, which uses cross products of the angular velocities and the position vectors from point to point to calculate the velocities for each point. These velocities are then rotated into a common base frame and summed to generate a total velocity term for each point of interest. For simpler manipulators, like the one described here, it is faster and easier to simply take a time derivative of the position vector.
|
||||
|
||||
The velocity of the end-effector via time derivation of the position vector $^{O}P_{ae}$ is
|
||||
|
||||
|
||||
|
||||
Notice that the velocity terms in that vector can be easily separated into coefficients of the joint angular velocities, $\dot{q1}$ and $\dot{q2}$. Unlike the position vector, which has joint angle dependencies scattered throughout and can't be separated out into neat pieces (i.e. it's highly nonlinear), the velocity vector can be broken into neat pieces.
|
||||
|
||||
The matrix of form of the velocity equation is given as
|
||||
|
||||
|
||||
|
||||
|
||||
### The Jacobian
|
||||
|
||||
The matrix given above is known as the Jacobian
|
||||
|
||||
|
||||
|
||||
The Jacobian of a robotic system describes the mapping from angular joint velocities to linear end-effector velocities. Inverse kinematics relies on the calculation of the Jacobian and, more crucially, its inverse. With the Jacobian defined, you can now determine the required joint velocities at any configuration to generate a desired end-effector velocity with the equation
|
||||
|
||||
|
||||
|
||||
This equation forms the basis of the *resolved-rate controller* below, allowing the manipulator to move the end-effector in any desired direction as long as the Jacobian can be inverted. But what happens when a Jacobian *can't* be inverted?
|
||||
|
||||
### Singularities
|
||||
|
||||
Singularities occur when the Jacobian matrix can't be inverted. A singular matrix is one whose inverse's values go to infinity. This happens because the deteminant of the matrix goes to 0, and when you divide by the determinant to calculate the inverse everything goes *kablooey*. In robotics, singularities refer specifically to the Jacobian matrix defined above going singular. Robotic singularities occur when the desired velocity can't physically be commanded. In the case here, we are trying to control the end-effector velocity, and the Jacobian depends explicitly on the joint angles $q1$ and $q2$. That implies that a manipulator goes singular when it is in a configuration (i.e. the joints are at specific angles) that cannot produce some end-effector velocity, regardless of how high the joint velocities go. In practice, this typically occurs when the tip of the manipulator is either pushed all the way out to the workspace boundary (in the graphics client, this boundary is the red circle around the outside of the manipulator) or when it gets curled up on itself in a weird, pretzel-like way.
|
||||
|
||||
For simple manipulators like the one presented here, looking at the configurations that make the determinant of the jacobian go to zero will provide insight into the configurations that cause singularities. The determinant of the Jacobian is given as
|
||||
|
||||
|
||||
|
||||
|
||||
After a lot of trig, this can be whittled down to show that the manipulator is in a singular configuration when $q2 = 0$ or $q2 = 180$degrees. These positions correspond to the manipulator stretching all the way out to the workspace boundary or bending around a laying one joint on top of the other. In both cases, the manipulator is incapable of moving in the $x$-direction in frame A. You can see the effects of a singularity on the controller by moving the end-effector in the same direction until it reaches the workspace boundary. Once it reaches the boundary, there's really no telling what will happen.
|
||||
|
||||
Note that the determinant is not a good metric to measure *how close* a manipulator is to singularity, only when it's actually fully singular. Determinants don't monotonically approach zero as the manipulator approaches a singular configuration.
|
||||
|
||||
With a physical manipulator, singularities are very dangerous. If they are not handled and prevented, the controller will demand infinite joint rates out of the manipulator. Since real motors can't provide infinite rates, they all just spin in whatever direction as fast and as hard as they can. This causes the manipulator to fling itself about wildly, smashing whatever happens to be in the workspace with it. This is why all manipulators should have caution tape on the floor around them in a perimeter a bit outside their workspace, so if the manipulator *does* go singular it can't reach anybody. More manipulators are damaged or destroyed by unmanaged singularites than by aliens, lasers, and monsters combined.
|
||||
|
||||
## Control Modes
|
||||
|
||||
There are three control modes implemented in the simulation and are detailed below. All of these are fairly standard and give a good idea of how to implement robotic controllers in Trick. There are no joint limits implemented in the sim as there would be in a real robot.
|
||||
|
||||
### Single Joint Mode
|
||||
**Single Joint Mode** controls the joint velocity of a single joint, plain and simple. On the gui, select **SJoint** on the "Robot Mode" section and then you can select the joint with the select arrow buttons and drag the rate slider either way. Because there are no *inverse kinematics* involved (we're just applying a joint rate) this is the only mode inmplemented where singularities are not a concern. Singularities only occur when the required joint rates to produce a desired motion of a point of interest are calculated, and here we're just demanding pure joint rates.
|
||||
|
||||
|
||||
### Manual Mode
|
||||
**Manual Mode** moves the end-effector with a particular *velocity* (direction and speed). The control interface for this mode is the "EE Velocity" portion with the gray circle. Select **Manual** in the "Robot Mode" section. Then by clicking anywhere in the cirle, you apply that end-effector velocity to the manipulator (the velocity is described in frame O, the base frame). Points selected closer to the center of the circle will move the end-effector slower, and points closer to the edge of the circle move it faster. The direction of motion is determined by the angle from the center of circle.
|
||||
|
||||
In this mode, you *do* have to watch out for singularities because the inverse Jacobian is being calculated to generate the required joint rates to produce the end-effector velocity you choose. You'll know when you hit one.
|
||||
|
||||
The required joint rates are determined by the formula mentioned in the Jacobian section
|
||||
|
||||
|
||||
|
||||
where the end-effector velocity terms are now the *desired* end-effector velocities.
|
||||
|
||||
### End-Effector Position Mode
|
||||
**End-Effector Position Mode** takes a desired end-effector *position* and commands the manipulator to move towards it. It does this iteratively, moving incrementally every command cycle closer to the point and checking its position with respect to the desired position. Once the end-effector is close enough to the desired position, it stops moving and congratulates itself on a job well done.
|
||||
|
||||
The control interface for this mode is actually the workspace of the manipulator in the graphics client. Select **EEPos** from the "Robot Mode" section of the gui and click anywhere in or near the workspace. A dot will appear indicating the desired location of the end-effector, and the manipulator will move that direction. This is another mode impacted by singularities, and forcing the manipulator to either reach outside the workspace or put the end-effector on the base origin will result in fireworks. It's especially noticable if you turn on **Trace** before you try to break it.
|
||||
|
||||
The methodolgy behind this is actually a combination of a PD[^1] controller and Manual Mode. The difference is calculated between the desired and current end-effector position
|
||||
|
||||
|
||||
|
||||
This is used as the proportional offset in a PD controller with some gain $K_P$. To act as a mitigating control, the desired velocity is defined to be 0. This allows a smoother motion near the goal, as any end-effector velocity acts as a drag. The PD control formulation ends up looking like
|
||||
|
||||
|
||||
|
||||
These desired velocities are then fed into the same Manual Mode controller, and the position offset is updated every cycle until it is "close enough."
|
||||
|
||||
[^1]: PD controllers are a subset of the broader PID controller implementation. PID controllers are very common and are used in a wide variety of situations. There are mountains of articles explaining what they are and how they work to be found in the wilds of the internet. PID controllers have proportional (P), integral (I), and derivative (D) components. Robotic manipulators often don't employ the integral component because it is ill-suited to systems whose desired control point never sits still, which is why the controller implemented here is simply a PD controller instead.
|
||||
|
||||
### Singularity Management
|
||||
|
||||
By default, the simulation checks for singularities and cancels the motion that would result in a singularity (and takes the manipulator out of mode). However, if you want to see the consequences of maneuvering through a singularity, it can be disabled.
|
||||
|
||||
In the input file RUN_2DPlanar/input.py, add this line at the bottom
|
||||
|
||||
Manip2D.robot.kinemat.checkSingularities = False
|
||||
|
||||
For maximum effect, turn on **Trace** while it does this.
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
33
trick_sims/SIM_robot/RUN_2DPlanar/input.py
Normal file
33
trick_sims/SIM_robot/RUN_2DPlanar/input.py
Normal file
@ -0,0 +1,33 @@
|
||||
exec(open("./Modified_data/realtime.py").read())
|
||||
|
||||
# Variable Server Data should be copied at top of frame.
|
||||
trick.var_set_copy_mode(2)
|
||||
|
||||
Manip2D.robot.kinemat.joint_q[0] = trick.attach_units("degrees",45.0)
|
||||
Manip2D.robot.kinemat.joint_q[1] = trick.attach_units("degrees",45.0)
|
||||
|
||||
Manip2D.robot.kinemat.joint_w[0] = 0.0
|
||||
Manip2D.robot.kinemat.joint_w[1] = 0.0
|
||||
|
||||
Manip2D.robot.controller.Kp = 2.0
|
||||
Manip2D.robot.controller.Kd = 0.3
|
||||
Manip2D.robot.kinemat.checkSingularities = False
|
||||
|
||||
armIntegLoop.getIntegrator(trick.Euler, Manip2D.robot.ndof)
|
||||
|
||||
#==========================================
|
||||
# Start the Graphics Client
|
||||
#==========================================
|
||||
varServerPort = trick.var_server_get_port();
|
||||
RobotDisplay_path = "models/graphics/build/RobotDisplay.jar"
|
||||
|
||||
if (os.path.isfile(RobotDisplay_path)) :
|
||||
RobotDisplay_cmd = "java -jar " \
|
||||
+ RobotDisplay_path \
|
||||
+ " " + str(varServerPort) + " &" ;
|
||||
print(RobotDisplay_cmd)
|
||||
os.system( RobotDisplay_cmd);
|
||||
else :
|
||||
print('==================================================================================')
|
||||
print('RobotDisplay needs to be built. Please \"cd\" into ../models/graphics and type \"make\".')
|
||||
print('==================================================================================')
|
||||
32
trick_sims/SIM_robot/S_define
Normal file
32
trick_sims/SIM_robot/S_define
Normal file
@ -0,0 +1,32 @@
|
||||
/************************TRICK HEADER*************************
|
||||
PURPOSE:
|
||||
( Simulate kinematic, planar 2 degree-of-freedom manipulator )
|
||||
LIBRARY DEPENDENCIES:
|
||||
(
|
||||
(manipulator/manipulator.cc)
|
||||
)
|
||||
*************************************************************/
|
||||
|
||||
#include "sim_objects/default_trick_sys.sm"
|
||||
##include "include/trick/exec_proto.h"
|
||||
##include "manipulator/manipulator.hh"
|
||||
|
||||
class ManipulatorSimObject : public Trick::SimObject
|
||||
{
|
||||
|
||||
public:
|
||||
PlanarManip robot;
|
||||
|
||||
ManipulatorSimObject(int ndof): robot(ndof)
|
||||
{
|
||||
(0.050, "scheduled") robot.calcKinematics();
|
||||
(0.050, "scheduled") robot.control();
|
||||
("derivative") robot.stateDeriv();
|
||||
("integration") trick_ret = robot.updateState();
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
ManipulatorSimObject Manip2D(2);
|
||||
|
||||
IntegLoop armIntegLoop(0.050) Manip2D;
|
||||
14
trick_sims/SIM_robot/S_overrides.mk
Normal file
14
trick_sims/SIM_robot/S_overrides.mk
Normal file
@ -0,0 +1,14 @@
|
||||
TRICK_CFLAGS += -I./models -I${TRICK_HOME} -g -O0 -Wall -Wextra -Wshadow
|
||||
TRICK_CXXFLAGS += -I./models -I${TRICK_HOME} -g -O0 -Wall -Wextra -Wshadow
|
||||
|
||||
.PHONY: clean_graphics
|
||||
|
||||
all: models/graphics/build/RobotDisplay.jar
|
||||
|
||||
spotless: clean_graphics
|
||||
|
||||
models/graphics/build/RobotDisplay.jar:
|
||||
${MAKE} -C ./models/graphics
|
||||
|
||||
clean_graphics:
|
||||
${MAKE} -C ./models/graphics clean
|
||||
BIN
trick_sims/SIM_robot/images/Angles.png
Executable file
BIN
trick_sims/SIM_robot/images/Angles.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 147 KiB |
BIN
trick_sims/SIM_robot/images/Frames.png
Executable file
BIN
trick_sims/SIM_robot/images/Frames.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 139 KiB |
BIN
trick_sims/SIM_robot/images/GraphicsClient.png
Executable file
BIN
trick_sims/SIM_robot/images/GraphicsClient.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 27 KiB |
BIN
trick_sims/SIM_robot/images/Jac.png
Executable file
BIN
trick_sims/SIM_robot/images/Jac.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 2.3 KiB |
BIN
trick_sims/SIM_robot/images/JacDet.png
Executable file
BIN
trick_sims/SIM_robot/images/JacDet.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 2.0 KiB |
BIN
trick_sims/SIM_robot/images/JacInv.png
Executable file
BIN
trick_sims/SIM_robot/images/JacInv.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 1.1 KiB |
BIN
trick_sims/SIM_robot/images/L1cos1.png
Executable file
BIN
trick_sims/SIM_robot/images/L1cos1.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 524 B |
BIN
trick_sims/SIM_robot/images/L1sin1.png
Executable file
BIN
trick_sims/SIM_robot/images/L1sin1.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 538 B |
BIN
trick_sims/SIM_robot/images/L2cos2.png
Executable file
BIN
trick_sims/SIM_robot/images/L2cos2.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 535 B |
BIN
trick_sims/SIM_robot/images/L2sin2.png
Executable file
BIN
trick_sims/SIM_robot/images/L2sin2.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 554 B |
BIN
trick_sims/SIM_robot/images/MatrixEq_RotMat.png
Executable file
BIN
trick_sims/SIM_robot/images/MatrixEq_RotMat.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 1.8 KiB |
BIN
trick_sims/SIM_robot/images/PDControl.png
Executable file
BIN
trick_sims/SIM_robot/images/PDControl.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
BIN
trick_sims/SIM_robot/images/POIs.png
Executable file
BIN
trick_sims/SIM_robot/images/POIs.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 134 KiB |
BIN
trick_sims/SIM_robot/images/P_B_E_Fa.png
Executable file
BIN
trick_sims/SIM_robot/images/P_B_E_Fa.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 1.7 KiB |
BIN
trick_sims/SIM_robot/images/P_B_E_Fb.png
Executable file
BIN
trick_sims/SIM_robot/images/P_B_E_Fb.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 1.6 KiB |
BIN
trick_sims/SIM_robot/images/P_ab_A.png
Executable file
BIN
trick_sims/SIM_robot/images/P_ab_A.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 856 B |
BIN
trick_sims/SIM_robot/images/P_be_B.png
Executable file
BIN
trick_sims/SIM_robot/images/P_be_B.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 881 B |
BIN
trick_sims/SIM_robot/images/Pab_full_xform_Fa.png
Executable file
BIN
trick_sims/SIM_robot/images/Pab_full_xform_Fa.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 3.0 KiB |
BIN
trick_sims/SIM_robot/images/Pab_full_xform_O.png
Executable file
BIN
trick_sims/SIM_robot/images/Pab_full_xform_O.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 2.9 KiB |
BIN
trick_sims/SIM_robot/images/Pae_F_A.png
Executable file
BIN
trick_sims/SIM_robot/images/Pae_F_A.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 1.9 KiB |
BIN
trick_sims/SIM_robot/images/Pae_O.png
Executable file
BIN
trick_sims/SIM_robot/images/Pae_O.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 1.8 KiB |
BIN
trick_sims/SIM_robot/images/Pae_Pab_Pbe.png
Executable file
BIN
trick_sims/SIM_robot/images/Pae_Pab_Pbe.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 145 KiB |
BIN
trick_sims/SIM_robot/images/Pae_addition.png
Executable file
BIN
trick_sims/SIM_robot/images/Pae_addition.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 2.8 KiB |
BIN
trick_sims/SIM_robot/images/Pbe_F_A.png
Executable file
BIN
trick_sims/SIM_robot/images/Pbe_F_A.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 2.3 KiB |
BIN
trick_sims/SIM_robot/images/Pbe_O.png
Executable file
BIN
trick_sims/SIM_robot/images/Pbe_O.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 2.1 KiB |
BIN
trick_sims/SIM_robot/images/Pbe_double_rotation.png
Executable file
BIN
trick_sims/SIM_robot/images/Pbe_double_rotation.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 1.1 KiB |
BIN
trick_sims/SIM_robot/images/Pbe_single_rotation.png
Executable file
BIN
trick_sims/SIM_robot/images/Pbe_single_rotation.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 3.5 KiB |
BIN
trick_sims/SIM_robot/images/R_A_B.png
Executable file
BIN
trick_sims/SIM_robot/images/R_A_B.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 1.4 KiB |
BIN
trick_sims/SIM_robot/images/R_B_A.png
Executable file
BIN
trick_sims/SIM_robot/images/R_B_A.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 357 B |
BIN
trick_sims/SIM_robot/images/RotMat.png
Executable file
BIN
trick_sims/SIM_robot/images/RotMat.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 10 KiB |
BIN
trick_sims/SIM_robot/images/UnitVecComps.png
Executable file
BIN
trick_sims/SIM_robot/images/UnitVecComps.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 24 KiB |
BIN
trick_sims/SIM_robot/images/V_matrixform.png
Executable file
BIN
trick_sims/SIM_robot/images/V_matrixform.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 2.9 KiB |
BIN
trick_sims/SIM_robot/images/Vec_B_to_A.png
Executable file
BIN
trick_sims/SIM_robot/images/Vec_B_to_A.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 998 B |
BIN
trick_sims/SIM_robot/images/VectorComponents.png
Executable file
BIN
trick_sims/SIM_robot/images/VectorComponents.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 238 KiB |
BIN
trick_sims/SIM_robot/images/Vel_ee.png
Executable file
BIN
trick_sims/SIM_robot/images/Vel_ee.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 2.7 KiB |
BIN
trick_sims/SIM_robot/images/XhatBEq.png
Executable file
BIN
trick_sims/SIM_robot/images/XhatBEq.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 1.0 KiB |
BIN
trick_sims/SIM_robot/images/YhatBEq.png
Executable file
BIN
trick_sims/SIM_robot/images/YhatBEq.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 1.0 KiB |
BIN
trick_sims/SIM_robot/images/cos_unitvec.png
Executable file
BIN
trick_sims/SIM_robot/images/cos_unitvec.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 690 B |
BIN
trick_sims/SIM_robot/images/cosq.png
Executable file
BIN
trick_sims/SIM_robot/images/cosq.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 410 B |
BIN
trick_sims/SIM_robot/images/deltaX.png
Executable file
BIN
trick_sims/SIM_robot/images/deltaX.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 608 B |
BIN
trick_sims/SIM_robot/images/orthonormality.png
Executable file
BIN
trick_sims/SIM_robot/images/orthonormality.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 746 B |
BIN
trick_sims/SIM_robot/images/sinq.png
Executable file
BIN
trick_sims/SIM_robot/images/sinq.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 424 B |
BIN
trick_sims/SIM_robot/images/x_y_Fb.png
Executable file
BIN
trick_sims/SIM_robot/images/x_y_Fb.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 913 B |
BIN
trick_sims/SIM_robot/images/x_y_Fe.png
Executable file
BIN
trick_sims/SIM_robot/images/x_y_Fe.png
Executable file
Binary file not shown.
|
After Width: | Height: | Size: 933 B |
125
trick_sims/SIM_robot/models/control/ManipControl.cc
Normal file
125
trick_sims/SIM_robot/models/control/ManipControl.cc
Normal file
@ -0,0 +1,125 @@
|
||||
/********************************* TRICK HEADER *******************************
|
||||
PURPOSE: ( Control algorithms for 2-dof planar manip.)
|
||||
LIBRARY DEPENDENCY:
|
||||
((control/ManipControl.o)
|
||||
(utils/utils.o)
|
||||
)
|
||||
PROGRAMMERS:
|
||||
(((Sean Harmeyer) (NASA) (Dec 2022) (Trick Example Sim)))
|
||||
*******************************************************************************/
|
||||
|
||||
#include <cmath>
|
||||
#include <algorithm>
|
||||
#include "control/ManipControl.hh"
|
||||
|
||||
ManipControl::ManipControl(int numDof):
|
||||
ndof(numDof)
|
||||
{
|
||||
|
||||
manualFrame = Task;
|
||||
commandedJointRate[0] = 0.0;
|
||||
commandedJointRate[1] = 0.0;
|
||||
singleJointNum = 0;
|
||||
singleJointRate = 0;
|
||||
desiredPos[0] = 2;
|
||||
desiredPos[1] = 0;
|
||||
|
||||
Kp = 2.0;
|
||||
Kd = 0.3;
|
||||
|
||||
posEps = .01;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void ManipControl::manualControl()
|
||||
{
|
||||
int i = 0;
|
||||
|
||||
/* command_ee_rate should be input from the user and will be converted to
|
||||
* motion in the Task Frame */
|
||||
if(manualFrame == EE)
|
||||
{
|
||||
/* In the EE control frame, the commanded manual rate needs to be rotated
|
||||
* from the EE frame to the Task frame */
|
||||
utils.MVMult(commandedEERate_Task,R_ee_task,manualCommandRate);
|
||||
}
|
||||
else
|
||||
{
|
||||
/* In the Task control frame, the commanded manual rate is already in the Task
|
||||
* Frame, so no rotation required */
|
||||
for(i=0;i<ndof;i++)
|
||||
commandedEERate_Task[i] = manualCommandRate[i];
|
||||
}
|
||||
|
||||
calcResolvedJointRates();
|
||||
|
||||
}
|
||||
|
||||
void ManipControl::calcResolvedJointRates()
|
||||
{
|
||||
utils.MVMult(commandedJointRate,jacInv,commandedEERate_Task);
|
||||
}
|
||||
|
||||
void ManipControl::singleJointControl()
|
||||
{
|
||||
/* Drive the selected joint at the commanded rate */
|
||||
commandedJointRate[singleJointNum] = singleJointRate;
|
||||
|
||||
}
|
||||
|
||||
|
||||
bool ManipControl::EEPositionAuto(double *curPos, double *curVel)
|
||||
{
|
||||
|
||||
/* Returns true when desired EE position has been reached, false otherwise */
|
||||
|
||||
double posErr[2];
|
||||
int i;
|
||||
bool withinTol[ndof];
|
||||
bool posReached = true;
|
||||
|
||||
for(i=0;i<ndof;i++)
|
||||
{
|
||||
withinTol[i] = false;
|
||||
posErr[i] = desiredPos[i] - curPos[i];
|
||||
if( fabs(posErr[i]) <= posEps )
|
||||
{
|
||||
withinTol[i] = true;
|
||||
}
|
||||
|
||||
commandedEERate_Task[i] = Kp*posErr[i] - Kd*curVel[i];
|
||||
|
||||
}
|
||||
|
||||
for(i=0;i<ndof;i++)
|
||||
{
|
||||
if( !withinTol[i] )
|
||||
{
|
||||
calcResolvedJointRates();
|
||||
posReached = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return(posReached);
|
||||
|
||||
|
||||
}
|
||||
|
||||
void ManipControl::clearControlCommands()
|
||||
{
|
||||
singleJointNum = 0;
|
||||
singleJointRate = 0;
|
||||
|
||||
int i = 0;
|
||||
|
||||
for(i=0;i<ndof;i++)
|
||||
{
|
||||
manualCommandRate[i] = 0;
|
||||
desiredPos[i] = 0;
|
||||
commandedEERate_Task[i] = 0;
|
||||
commandedJointRate[i] = 0;
|
||||
|
||||
}
|
||||
}
|
||||
65
trick_sims/SIM_robot/models/control/ManipControl.hh
Normal file
65
trick_sims/SIM_robot/models/control/ManipControl.hh
Normal file
@ -0,0 +1,65 @@
|
||||
#ifndef __MANIPCONTROL_HH_
|
||||
#define __MANIPCONTROL_HH_
|
||||
/**************************************************************************
|
||||
PURPOSE: (2D Manipulator class definitions including kinematics and control)
|
||||
***************************************************************************/
|
||||
|
||||
#include "kinematics/ManipKinemat.hh"
|
||||
#include "utils/utils.hh"
|
||||
|
||||
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
enum ControlFrame
|
||||
{
|
||||
Task, /* Manual control velocities in the Task Frame */
|
||||
EE /* Manual control velocities in the EE's own frame */
|
||||
};
|
||||
|
||||
|
||||
class ManipControl
|
||||
{
|
||||
public:
|
||||
|
||||
ControlFrame manualFrame; /* -- Frame for Manual Control */
|
||||
|
||||
/* Single Joint Control Mode data */
|
||||
int singleJointNum; /* -- SingleJoint control joint selection */
|
||||
double singleJointRate; /* rad/s SingleJoint control joint rate for selected joint */
|
||||
|
||||
/* Manual Control Mode data */
|
||||
double manualCommandRate[2]; /* m/s command rate of EE in manual mode */
|
||||
|
||||
/* EEPos Control Mode data */
|
||||
double desiredPos[2]; /* m Desired EE position in TaskFrame of EE for EEPos control mode */
|
||||
double Kp; /* -- Proportional gain for EEPos tracking */
|
||||
double Kd; /* -- Derivative gain for EEPos tracking */
|
||||
|
||||
ManipUtils utils; /* -- math utilities */
|
||||
|
||||
double commandedJointRate[2]; /* rad/s calculated required joint rates */
|
||||
|
||||
/* Information required from kinematics */
|
||||
double **jacInv; /* -- inverse Jacobian as calculated by kinematics */
|
||||
double **R_ee_task; /* -- rot matrix from EE to task frame */
|
||||
|
||||
void manualControl();
|
||||
void singleJointControl();
|
||||
bool EEPositionAuto(double *pos, double *vel);
|
||||
void clearControlCommands();
|
||||
|
||||
ManipControl(int numdof);
|
||||
|
||||
private:
|
||||
|
||||
int ndof; /* -- number of degrees of freedom */
|
||||
|
||||
double commandedEERate_Task[2]; /* m/s command rate of EE in Task frame */
|
||||
|
||||
double posEps; /* m error tolerance for EEPosAuto convergence */
|
||||
|
||||
void calcResolvedJointRates(); /* -- calculate resolved joint rates for EE motion */
|
||||
};
|
||||
|
||||
#endif
|
||||
1
trick_sims/SIM_robot/models/graphics/.gitignore
vendored
Normal file
1
trick_sims/SIM_robot/models/graphics/.gitignore
vendored
Normal file
@ -0,0 +1 @@
|
||||
!\makefile
|
||||
6
trick_sims/SIM_robot/models/graphics/Makefile
Executable file
6
trick_sims/SIM_robot/models/graphics/Makefile
Executable file
@ -0,0 +1,6 @@
|
||||
|
||||
all:
|
||||
mvn package
|
||||
|
||||
clean:
|
||||
rm -rf build
|
||||
121
trick_sims/SIM_robot/models/graphics/pom.xml
Normal file
121
trick_sims/SIM_robot/models/graphics/pom.xml
Normal file
@ -0,0 +1,121 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
|
||||
<project xmlns="http://maven.apache.org/POM/4.0.0" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
|
||||
xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd">
|
||||
<modelVersion>4.0.0</modelVersion>
|
||||
|
||||
<groupId>trick-java</groupId>
|
||||
<artifactId>trick-java</artifactId>
|
||||
<version>23.0.0-beta</version>
|
||||
|
||||
<name>trick-java</name>
|
||||
|
||||
<url>https://github.com/nasa/trick</url>
|
||||
|
||||
<properties>
|
||||
<project.build.sourceEncoding>UTF-8</project.build.sourceEncoding>
|
||||
<maven.compiler.source>1.8</maven.compiler.source>
|
||||
<maven.compiler.target>1.8</maven.compiler.target>
|
||||
</properties>
|
||||
|
||||
<dependencies>
|
||||
<dependency>
|
||||
<groupId>junit</groupId>
|
||||
<artifactId>junit</artifactId>
|
||||
<version>4.13.1</version>
|
||||
<scope>test</scope>
|
||||
</dependency>
|
||||
</dependencies>
|
||||
|
||||
<build>
|
||||
|
||||
<finalName>RobotDisplay</finalName>
|
||||
|
||||
<directory>build</directory>
|
||||
<plugins>
|
||||
<plugin>
|
||||
<groupId>org.apache.maven.plugins</groupId>
|
||||
<artifactId>maven-javadoc-plugin</artifactId>
|
||||
<version>3.1.1</version>
|
||||
<configuration>
|
||||
<javadocExecutable>${java.home}/bin/javadoc</javadocExecutable>
|
||||
<destDir>../../share/doc/trick/java</destDir>
|
||||
</configuration>
|
||||
</plugin>
|
||||
</plugins>
|
||||
|
||||
<pluginManagement><!-- lock down plugins versions to avoid using Maven defaults (may be moved to parent pom) -->
|
||||
|
||||
<plugins>
|
||||
<!-- clean lifecycle, see https://maven.apache.org/ref/current/maven-core/lifecycles.html#clean_Lifecycle -->
|
||||
<plugin>
|
||||
<artifactId>maven-clean-plugin</artifactId>
|
||||
<version>3.1.0</version>
|
||||
</plugin>
|
||||
|
||||
<!-- default lifecycle, jar packaging: see https://maven.apache.org/ref/current/maven-core/default-bindings.html#Plugin_bindings_for_jar_packaging -->
|
||||
<plugin>
|
||||
<artifactId>maven-resources-plugin</artifactId>
|
||||
<version>3.0.2</version>
|
||||
</plugin>
|
||||
|
||||
<plugin>
|
||||
<artifactId>maven-compiler-plugin</artifactId>
|
||||
<version>3.8.0</version>
|
||||
<configuration>
|
||||
<compilerArgs>
|
||||
<arg>-g</arg>
|
||||
<arg>-Xlint:unchecked</arg>
|
||||
<arg>-Xlint:deprecation</arg>
|
||||
</compilerArgs>
|
||||
</configuration>
|
||||
</plugin>
|
||||
|
||||
<plugin>
|
||||
<!-- Build an executable JAR -->
|
||||
<groupId>org.apache.maven.plugins</groupId>
|
||||
<artifactId>maven-jar-plugin</artifactId>
|
||||
<version>3.1.0</version>
|
||||
<configuration>
|
||||
<archive>
|
||||
<manifest>
|
||||
<addClasspath>true</addClasspath>
|
||||
<classpathPrefix>lib/</classpathPrefix>
|
||||
<mainClass>RobotDisplay</mainClass>
|
||||
</manifest>
|
||||
</archive>
|
||||
</configuration>
|
||||
</plugin>
|
||||
|
||||
<plugin>
|
||||
<artifactId>maven-surefire-plugin</artifactId>
|
||||
<version>2.22.1</version>
|
||||
</plugin>
|
||||
|
||||
<plugin>
|
||||
<artifactId>maven-install-plugin</artifactId>
|
||||
<version>2.5.2</version>
|
||||
</plugin>
|
||||
|
||||
<plugin>
|
||||
<artifactId>maven-deploy-plugin</artifactId>
|
||||
<version>2.8.2</version>
|
||||
</plugin>
|
||||
|
||||
<!-- site lifecycle, see https://maven.apache.org/ref/current/maven-core/lifecycles.html#site_Lifecycle -->
|
||||
<plugin>
|
||||
<artifactId>maven-site-plugin</artifactId>
|
||||
<version>3.7.1</version>
|
||||
</plugin>
|
||||
|
||||
<!--
|
||||
<plugin>
|
||||
<artifactId>maven-project-info-reports-plugin</artifactId>
|
||||
<version>3.0.0</version>
|
||||
</plugin>
|
||||
-->
|
||||
|
||||
</plugins>
|
||||
</pluginManagement>
|
||||
</build>
|
||||
</project>
|
||||
@ -0,0 +1,977 @@
|
||||
/*
|
||||
* Trick
|
||||
* 2023 (c) National Aeronautics and Space Administration (NASA)
|
||||
*/
|
||||
import java.awt.*;
|
||||
import java.awt.event.MouseEvent;
|
||||
import javax.swing.event.MouseInputAdapter;
|
||||
import java.awt.event.ActionEvent;
|
||||
import java.awt.event.ActionListener;
|
||||
import java.io.BufferedOutputStream;
|
||||
import java.io.BufferedReader;
|
||||
import java.io.DataOutputStream;
|
||||
import java.io.IOException;
|
||||
import java.io.InputStreamReader;
|
||||
import java.net.Socket;
|
||||
import java.util.*;
|
||||
import javax.swing.BoxLayout;
|
||||
import javax.swing.JButton;
|
||||
import javax.swing.JFrame;
|
||||
import javax.swing.JLabel;
|
||||
import javax.swing.JPanel;
|
||||
import javax.swing.JSlider;
|
||||
import javax.swing.event.ChangeEvent;
|
||||
import javax.swing.event.ChangeListener;
|
||||
import javax.swing.BorderFactory;
|
||||
import javax.swing.border.EtchedBorder;
|
||||
import java.awt.Component;
|
||||
/**
|
||||
* @author penn
|
||||
*/
|
||||
//------------------------------------------------------------------------------
|
||||
class RobotCtrlMode {
|
||||
public static final int NOPE = 0;
|
||||
public static final int CANCEL = 1;
|
||||
public static final int SINGLE_JOINT = 2;
|
||||
public static final int MANUAL = 3;
|
||||
public static final int EEPOS = 4;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
class Link {
|
||||
public double l; // length of the link (meters).
|
||||
public double q; // joint angle (radians).
|
||||
|
||||
public Link() {
|
||||
l = 1.0;
|
||||
q = 0.0;
|
||||
}
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
class ScenePoly {
|
||||
public Color color;
|
||||
public int n;
|
||||
public double[] x;
|
||||
public double[] y;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
class Point {
|
||||
public double x;
|
||||
public double y;
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
class PathTrace {
|
||||
int capacity;
|
||||
public Point[] path;
|
||||
public int head;
|
||||
public int tail;
|
||||
public PathTrace() {
|
||||
capacity = 1000;
|
||||
path = new Point[capacity];
|
||||
head = 0;
|
||||
tail = 0;
|
||||
}
|
||||
public void insert (double x, double y) {
|
||||
if (path[tail] == null) {
|
||||
path[tail] = new Point();
|
||||
}
|
||||
path[tail].x = x;
|
||||
path[tail].y = y;
|
||||
tail = (tail + 1) % capacity;
|
||||
if (tail == head) head = (head + 1) % capacity;
|
||||
}
|
||||
public void clear() {
|
||||
head = tail = 0;
|
||||
}
|
||||
public int count () {
|
||||
if (head == tail) return 0;
|
||||
if (head < tail) {
|
||||
return (tail - head);
|
||||
} else {
|
||||
return (tail - head + capacity);
|
||||
}
|
||||
}
|
||||
public int advance (int i ) {
|
||||
i = (i + 1) % capacity;
|
||||
return i;
|
||||
}
|
||||
public int begin () { return head; }
|
||||
public int end () { return tail; }
|
||||
public Point get(int i) {
|
||||
return path[i];
|
||||
}
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
class RangeView extends JPanel {
|
||||
private int scale;
|
||||
private Color selectedJointColor;
|
||||
private Color jointColor;
|
||||
private Color armColor;
|
||||
private Color maxReachColor;
|
||||
private Color desiredPosColor;
|
||||
private Color pathTraceColor;
|
||||
|
||||
private int worldOriginX;
|
||||
private int worldOriginY;
|
||||
private int[] workPolyX, workPolyY;
|
||||
private ScenePoly link_arm;
|
||||
|
||||
public int nlinks;
|
||||
public Link[] links;
|
||||
public PathTrace pathTrace;
|
||||
public boolean pathTraceEnabled;
|
||||
private int selectedJoint;
|
||||
public Point desiredPosition;
|
||||
|
||||
public RangeView( int mapScale, int numberOfLinks) {
|
||||
|
||||
setScale(mapScale);
|
||||
setToolTipText("Click mouse to set desired end-effector position.");
|
||||
|
||||
selectedJointColor = new Color(250,203,66);
|
||||
jointColor = Color.GRAY;
|
||||
armColor = Color.LIGHT_GRAY;
|
||||
maxReachColor = Color.RED;
|
||||
pathTraceColor = new Color(85,20,230);
|
||||
desiredPosColor = Color.BLUE;
|
||||
|
||||
workPolyX = new int[30];
|
||||
workPolyY = new int[30];
|
||||
|
||||
link_arm = new ScenePoly();
|
||||
link_arm.color = armColor;
|
||||
link_arm.n = 4;
|
||||
link_arm.x = new double[4];
|
||||
link_arm.y = new double[4];
|
||||
|
||||
nlinks = numberOfLinks;
|
||||
links = new Link[nlinks];
|
||||
for (int ii=0 ; ii<nlinks ; ii++) {
|
||||
links[ii] = new Link();
|
||||
}
|
||||
|
||||
pathTrace = new PathTrace();
|
||||
pathTraceEnabled = false;
|
||||
|
||||
selectedJoint = 0;
|
||||
|
||||
desiredPosition = new Point();
|
||||
desiredPosition.x = -0.5;
|
||||
desiredPosition.y = -0.5;
|
||||
|
||||
ViewListener viewListener = new ViewListener();
|
||||
addMouseListener(viewListener);
|
||||
addMouseMotionListener(viewListener);
|
||||
|
||||
}
|
||||
|
||||
private class ViewListener extends MouseInputAdapter {
|
||||
public void mouseReleased(MouseEvent e) {
|
||||
desiredPosition.x = (double)(e.getX() - worldOriginX) / (double)scale;
|
||||
desiredPosition.y = -(double)(e.getY() - worldOriginY) / (double)scale;
|
||||
repaint();
|
||||
}
|
||||
}
|
||||
|
||||
public void drawScenePoly(Graphics2D g, ScenePoly p, double angle_r ,
|
||||
double x, double y) {
|
||||
for (int ii = 0; ii < p.n; ii++) {
|
||||
workPolyX[ii] = (int)(worldOriginX + scale *
|
||||
( Math.cos(angle_r) * p.x[ii] - Math.sin(angle_r) * p.y[ii] + x));
|
||||
workPolyY[ii] = (int)(worldOriginY - scale *
|
||||
( Math.sin(angle_r) * p.x[ii] + Math.cos(angle_r) * p.y[ii] + y));
|
||||
}
|
||||
g.setPaint(p.color);
|
||||
g.fillPolygon(workPolyX, workPolyY, p.n);
|
||||
}
|
||||
|
||||
public void setScale (int mapScale) {
|
||||
if (mapScale < 2) {
|
||||
scale = 2;
|
||||
} else if (mapScale > 512) {
|
||||
scale = 512;
|
||||
} else {
|
||||
scale = mapScale;
|
||||
}
|
||||
repaint();
|
||||
}
|
||||
|
||||
public int getScale() {
|
||||
return scale;
|
||||
}
|
||||
|
||||
public void setSelectedJoint (int which) {
|
||||
selectedJoint = which;
|
||||
repaint();
|
||||
}
|
||||
|
||||
public void drawCenteredCircle(Graphics2D g, int x, int y, int d) {
|
||||
x = x-(d/2);
|
||||
y = y-(d/2);
|
||||
g.drawOval(x,y,d,d);
|
||||
}
|
||||
|
||||
public void fillCenteredCircle(Graphics2D g, int x, int y, int d) {
|
||||
x = x-(d/2);
|
||||
y = y-(d/2);
|
||||
g.fillOval(x,y,d,d);
|
||||
}
|
||||
|
||||
private void doDrawing(Graphics g) {
|
||||
Graphics2D g2d = (Graphics2D) g;
|
||||
|
||||
RenderingHints rh = new RenderingHints(
|
||||
RenderingHints.KEY_ANTIALIASING,
|
||||
RenderingHints.VALUE_ANTIALIAS_ON);
|
||||
|
||||
rh.put(RenderingHints.KEY_RENDERING,
|
||||
RenderingHints.VALUE_RENDER_QUALITY);
|
||||
|
||||
int width = getWidth();
|
||||
int height = getHeight();
|
||||
|
||||
worldOriginX = (width/2);
|
||||
worldOriginY = (height/2);
|
||||
|
||||
// ---------------------------------------------------------------------
|
||||
// Draw Robot
|
||||
double q_sum = 0.0;
|
||||
double l_sum = 0.0;
|
||||
double base_x = 0.0;
|
||||
double base_y = 0.0;
|
||||
double end_x = 0.0;
|
||||
double end_y = 0.0;
|
||||
|
||||
double base_width;
|
||||
double end_width;
|
||||
|
||||
// Find the total length of the links
|
||||
double extended_length = 0.0;
|
||||
for (int ii=0 ; ii<nlinks ; ii++) {
|
||||
extended_length += links[ii].l;
|
||||
}
|
||||
base_width = 0.10 * extended_length;
|
||||
|
||||
g2d.setPaint(maxReachColor);
|
||||
drawCenteredCircle(g2d,
|
||||
(int)(worldOriginX),
|
||||
(int)(worldOriginY),
|
||||
(int)(scale * 2 * extended_length )
|
||||
);
|
||||
|
||||
g2d.setPaint(desiredPosColor);
|
||||
fillCenteredCircle(g2d,
|
||||
(int)(worldOriginX + scale * desiredPosition.x),
|
||||
(int)(worldOriginY - scale * desiredPosition.y),
|
||||
(int)(scale * 0.2));
|
||||
|
||||
for (int ii=0 ; ii<nlinks ; ii++) {
|
||||
q_sum += links[ii].q;
|
||||
l_sum += links[ii].l;
|
||||
|
||||
// Calculate the width of the current link.
|
||||
end_width = 0.10 * (extended_length - l_sum);
|
||||
|
||||
// Calculate the position of the end of the current link.
|
||||
end_x = base_x + links[ii].l * Math.cos(q_sum);
|
||||
end_y = base_y + links[ii].l * Math.sin(q_sum);
|
||||
|
||||
link_arm.x[0] = 0.0; link_arm.y[0] = base_width/2.0;
|
||||
link_arm.x[1] = links[ii].l; link_arm.y[1] = base_width/2.0;
|
||||
link_arm.x[2] = links[ii].l; link_arm.y[2] = -base_width/2.0;
|
||||
link_arm.x[3] = 0.0; link_arm.y[3] = -base_width/2.0;
|
||||
|
||||
drawScenePoly(g2d, link_arm, q_sum, base_x, base_y);
|
||||
|
||||
if (ii == selectedJoint) {
|
||||
g2d.setPaint(selectedJointColor);
|
||||
} else {
|
||||
g2d.setPaint(jointColor);
|
||||
}
|
||||
|
||||
fillCenteredCircle(g2d,
|
||||
(int)(worldOriginX + scale * base_x),
|
||||
(int)(worldOriginY - scale * base_y),
|
||||
(int)(scale * 1.1 * base_width )
|
||||
);
|
||||
|
||||
g2d.setPaint(jointColor);
|
||||
fillCenteredCircle(g2d,
|
||||
(int)(worldOriginX + scale * base_x),
|
||||
(int)(worldOriginY - scale * base_y),
|
||||
(int)(scale * 0.8 * base_width )
|
||||
);
|
||||
|
||||
// The base position of the next link is the end position of the
|
||||
// current link.
|
||||
base_x = end_x;
|
||||
base_y = end_y;
|
||||
base_width = end_width;
|
||||
}
|
||||
|
||||
if (pathTraceEnabled) {
|
||||
// Save <end_x, end_y> into the path tracing array (circular).
|
||||
pathTrace.insert(end_x, end_y);
|
||||
// Render the end-effector path trace.
|
||||
if ( pathTrace.count() > 1 ) {
|
||||
int x1, y1, x2, y2;
|
||||
g2d.setPaint(pathTraceColor);
|
||||
int i = pathTrace.begin();
|
||||
Point p1 = pathTrace.get(i);
|
||||
x1 = (int)(worldOriginX + scale * p1.x);
|
||||
y1 = (int)(worldOriginY - scale * p1.y);
|
||||
for (i = pathTrace.advance(i) ;
|
||||
i != pathTrace.end() ;
|
||||
i = pathTrace.advance(i)) {
|
||||
Point p2 = pathTrace.get(i);
|
||||
x2 = (int)(worldOriginX + scale * p2.x);
|
||||
y2 = (int)(worldOriginY - scale * p2.y);
|
||||
g2d.drawLine(x1, y1, x2, y2);
|
||||
x1 = x2;
|
||||
y1 = y2;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Draw Information
|
||||
g2d.setPaint(Color.BLACK);
|
||||
g2d.drawString ( String.format(
|
||||
"SCALE: %d pixels/meter",scale), 20,20);
|
||||
g2d.drawString ( String.format(
|
||||
"Desired EE pos: <%.2f, %.2f>",desiredPosition.x,desiredPosition.y),20,40);
|
||||
g2d.drawString ( String.format(
|
||||
"Actual EE pos : <%.2f, %.2f>", end_x, end_y), 20,60);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void paintComponent(Graphics g) {
|
||||
super.paintComponent(g);
|
||||
doDrawing(g);
|
||||
}
|
||||
} // class RangeView
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
class VelocityCtrlPanel extends JPanel {
|
||||
private int mouse_x, mouse_y;
|
||||
private int padDiameter;
|
||||
private float pMag;
|
||||
private float nMag, vx, vy;
|
||||
public static final int padRadius = 50;
|
||||
|
||||
public VelocityCtrlPanel() {
|
||||
padDiameter = 2 * padRadius;
|
||||
clear();
|
||||
|
||||
ViewListener viewListener = new ViewListener();
|
||||
addMouseListener(viewListener);
|
||||
addMouseMotionListener(viewListener);
|
||||
|
||||
setPreferredSize(new Dimension(padDiameter, padDiameter));
|
||||
setMaximumSize( new Dimension(padDiameter, padDiameter));
|
||||
setToolTipText("Set end-effector velocity.");
|
||||
}
|
||||
@Override
|
||||
public void paintComponent(Graphics g) {
|
||||
super.paintComponent(g);
|
||||
Graphics2D g2d = (Graphics2D) g;
|
||||
g2d.setPaint(Color.LIGHT_GRAY);
|
||||
g2d.fillOval(0, 0, padDiameter, padDiameter);
|
||||
g2d.setPaint(Color.WHITE);
|
||||
g2d.fillOval(padRadius - (int)pMag,
|
||||
padRadius - (int)pMag,
|
||||
(int)(pMag*2),
|
||||
(int)(pMag*2));
|
||||
g2d.setPaint(Color.BLACK);
|
||||
g2d.drawLine(padRadius, padRadius, mouse_x, mouse_y);
|
||||
}
|
||||
private class ViewListener extends MouseInputAdapter {
|
||||
public void mouseReleased(MouseEvent e) {
|
||||
int x = e.getX() - padRadius;
|
||||
int y = e.getY() - padRadius;
|
||||
pMag = (float)Math.sqrt((double)(x*x+y*y));
|
||||
vx = (float)(x) / pMag;
|
||||
vy = (float)(y) / pMag;
|
||||
// Restrict the vector to the pad circle.
|
||||
if (pMag > padRadius) pMag = (float)(padRadius);
|
||||
// Normalize the vector magnitude to the range (0..1).
|
||||
nMag = pMag / (float)padRadius;
|
||||
mouse_x = (int)(vx * pMag) + padRadius;
|
||||
mouse_y = (int)(vy * pMag) + padRadius;
|
||||
repaint();
|
||||
}
|
||||
}
|
||||
public float getXvalue() { return (nMag * vx); }
|
||||
public float getYvalue() { return (nMag * -vy); }
|
||||
public float getMagnitude() { return nMag; }
|
||||
public void clear() {
|
||||
mouse_x = padRadius;
|
||||
mouse_y = padRadius;
|
||||
nMag = (float)0.0;
|
||||
vx = (float)0.0;
|
||||
vy = (float)0.0;
|
||||
repaint();
|
||||
}
|
||||
} // class VelocityCtrlPanel
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
class JointRatePanel extends JPanel implements ChangeListener {
|
||||
private int value;
|
||||
private JSlider degreesPerSecond;
|
||||
static final int DPS_MIN = -15;
|
||||
static final int DPS_MAX = 15;
|
||||
static final int DPS_INIT = 0;
|
||||
public JointRatePanel() {
|
||||
value = DPS_INIT;
|
||||
setLayout(new BoxLayout(this, BoxLayout.X_AXIS));
|
||||
setPreferredSize(new Dimension(200, 50));
|
||||
setMaximumSize( new Dimension(200, 50));
|
||||
degreesPerSecond =
|
||||
new JSlider(JSlider.HORIZONTAL, DPS_MIN, DPS_MAX, DPS_INIT);
|
||||
degreesPerSecond.addChangeListener(this);
|
||||
degreesPerSecond.setMajorTickSpacing(10);
|
||||
degreesPerSecond.setMinorTickSpacing(1);
|
||||
degreesPerSecond.setPaintTicks(true);
|
||||
degreesPerSecond.setPaintLabels(true);
|
||||
degreesPerSecond.setToolTipText("Joint Rate Selector");
|
||||
add(degreesPerSecond);
|
||||
}
|
||||
public void stateChanged(ChangeEvent e) {
|
||||
JSlider source = (JSlider)e.getSource();
|
||||
if (!source.getValueIsAdjusting()) {
|
||||
value = (int)source.getValue();
|
||||
System.out.println("Joint Rate : " + value);
|
||||
}
|
||||
}
|
||||
public double getValue() { return ((double)(value) * (Math.PI/180.0)) ; }
|
||||
public void setValue( int val) {
|
||||
value = val;
|
||||
degreesPerSecond.setValue( value);
|
||||
}
|
||||
public void clear() { setValue(0); }
|
||||
} // class JointRatePanel
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
class JointSelectPanel extends JPanel implements ActionListener {
|
||||
JLabel numberLabel;
|
||||
int count;
|
||||
int value;
|
||||
int desiredValue;
|
||||
|
||||
public JointSelectPanel(int nlinks) {
|
||||
count = nlinks;
|
||||
|
||||
setLayout(new BoxLayout(this, BoxLayout.X_AXIS));
|
||||
|
||||
JButton decButton = new JButton("\u25c0");
|
||||
decButton.addActionListener(this);
|
||||
decButton.setActionCommand("decrement");
|
||||
decButton.setToolTipText("Decrement");
|
||||
add(decButton);
|
||||
|
||||
numberLabel = new JLabel("");
|
||||
add(numberLabel);
|
||||
|
||||
JButton incButton = new JButton("\u25b6");
|
||||
incButton.addActionListener(this);
|
||||
incButton.setActionCommand("increment");
|
||||
incButton.setToolTipText("Increment");
|
||||
add(incButton);
|
||||
|
||||
setValue(0);
|
||||
}
|
||||
public void actionPerformed(ActionEvent e) {
|
||||
String s = e.getActionCommand();
|
||||
switch (s) {
|
||||
case "increment":
|
||||
// setValue( value + 1 );
|
||||
desiredValue = ( value + count + 1 ) % count;
|
||||
break;
|
||||
case "decrement":
|
||||
// setValue( value - 1 );
|
||||
desiredValue = ( value + count + 1 ) % count;
|
||||
break;
|
||||
default:
|
||||
System.out.println("Unknown Action Command:" + s);
|
||||
break;
|
||||
}
|
||||
}
|
||||
public int getValue() { return desiredValue; }
|
||||
public void setValue(int val) {
|
||||
// NOTE: the expression below looks weird because the result
|
||||
// of the Java mod operator isn't necessarily positive.
|
||||
value = ( (val % count) + count) % count;
|
||||
numberLabel.setText("" + value);
|
||||
}
|
||||
public void clear() {
|
||||
setValue(0);
|
||||
}
|
||||
} // class JointSelectPanel
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
class JointCtrlPanel extends JPanel {
|
||||
|
||||
public JointRatePanel jointRatePanel;
|
||||
public JointSelectPanel jointSelectPanel;
|
||||
|
||||
public JointCtrlPanel(int nlinks) {
|
||||
setLayout(new BoxLayout(this, BoxLayout.Y_AXIS));
|
||||
JLabel jointRateCtrlLabel = new JLabel("Joint Rate (deg/s)");
|
||||
jointRateCtrlLabel.setAlignmentX(Component.CENTER_ALIGNMENT);
|
||||
jointRatePanel = new JointRatePanel();
|
||||
JLabel jointSelectLabel = new JLabel("Joint Select");
|
||||
jointSelectLabel.setAlignmentX(Component.CENTER_ALIGNMENT);
|
||||
jointSelectPanel = new JointSelectPanel( nlinks);
|
||||
add(jointRateCtrlLabel);
|
||||
add(jointRatePanel);
|
||||
add(jointSelectLabel);
|
||||
add(jointSelectPanel);
|
||||
}
|
||||
} // class JointCtrlPanel
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
class ModeCtrlPanel extends JPanel implements ActionListener {
|
||||
|
||||
JButton eePosButton;
|
||||
JButton manualButton;
|
||||
JButton singleJointButton;
|
||||
JButton cancelButton;
|
||||
int desired_mode;
|
||||
|
||||
public ModeCtrlPanel() {
|
||||
|
||||
int desired_mode = RobotCtrlMode.NOPE;
|
||||
|
||||
setLayout(new BoxLayout(this, BoxLayout.Y_AXIS));
|
||||
//setAlignmentX(Component.CENTER_ALIGNMENT);
|
||||
|
||||
JLabel modeCtrlLabel = new JLabel("Robot Mode");
|
||||
modeCtrlLabel.setAlignmentX(Component.CENTER_ALIGNMENT);
|
||||
add(modeCtrlLabel);
|
||||
|
||||
JPanel modeSelectPanel = new JPanel();
|
||||
modeSelectPanel.setLayout(
|
||||
new BoxLayout(modeSelectPanel, BoxLayout.Y_AXIS));
|
||||
modeSelectPanel.setBorder(
|
||||
BorderFactory.createEtchedBorder(EtchedBorder.RAISED));
|
||||
modeSelectPanel.setAlignmentX(Component.CENTER_ALIGNMENT);
|
||||
|
||||
eePosButton = new JButton("EEPos");
|
||||
eePosButton.addActionListener(this);
|
||||
eePosButton.setActionCommand("eepos");
|
||||
eePosButton.setToolTipText("End-effector Position Mode");
|
||||
modeSelectPanel.add(eePosButton);
|
||||
|
||||
manualButton = new JButton("Manual");
|
||||
manualButton.addActionListener(this);
|
||||
manualButton.setActionCommand("manual");
|
||||
manualButton.setToolTipText("Manual Mode");
|
||||
modeSelectPanel.add(manualButton);
|
||||
|
||||
singleJointButton = new JButton("SJoint");
|
||||
singleJointButton.addActionListener(this);
|
||||
singleJointButton.setActionCommand("sjoint");
|
||||
singleJointButton.setToolTipText("Single Joint Mode");
|
||||
modeSelectPanel.add(singleJointButton);
|
||||
|
||||
add(modeSelectPanel);
|
||||
|
||||
cancelButton = new JButton("Cancel");
|
||||
cancelButton.addActionListener(this);
|
||||
cancelButton.setActionCommand("mode_cancel");
|
||||
cancelButton.setToolTipText("Cancel");
|
||||
cancelButton.setAlignmentX(Component.CENTER_ALIGNMENT);
|
||||
add(cancelButton);
|
||||
|
||||
}
|
||||
public void actionPerformed(ActionEvent e) {
|
||||
String s = e.getActionCommand();
|
||||
switch (s) {
|
||||
case "eepos":
|
||||
desired_mode = RobotCtrlMode.EEPOS;
|
||||
break;
|
||||
case "manual":
|
||||
desired_mode = RobotCtrlMode.MANUAL;
|
||||
break;
|
||||
case "sjoint":
|
||||
desired_mode = RobotCtrlMode.SINGLE_JOINT;
|
||||
break;
|
||||
case "mode_cancel":
|
||||
desired_mode = RobotCtrlMode.CANCEL;
|
||||
break;
|
||||
default:
|
||||
System.out.println("Unknown Action Command:" + s);
|
||||
break;
|
||||
}
|
||||
}
|
||||
public void clear() {
|
||||
// desired_mode = RobotCtrlMode.NOPE;
|
||||
eePosButton.setForeground(Color.BLACK);
|
||||
manualButton.setForeground(Color.BLACK);
|
||||
singleJointButton.setForeground(Color.BLACK);
|
||||
}
|
||||
public void setMode(int mode) {
|
||||
clear();
|
||||
if (mode == RobotCtrlMode.SINGLE_JOINT) {
|
||||
singleJointButton.setForeground(Color.GREEN);
|
||||
} else if (mode == RobotCtrlMode.MANUAL) {
|
||||
manualButton.setForeground(Color.GREEN);
|
||||
} else if (mode == RobotCtrlMode.EEPOS) {
|
||||
eePosButton.setForeground(Color.GREEN);
|
||||
}
|
||||
}
|
||||
public int desiredMode() {
|
||||
return desired_mode;
|
||||
}
|
||||
public void resetDesiredMode() {
|
||||
desired_mode = RobotCtrlMode.NOPE;
|
||||
}
|
||||
|
||||
} // class ModeCtrlPanel
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
class TraceCtrlPanel extends JPanel implements ActionListener {
|
||||
private RangeView rangeView;
|
||||
JButton traceButton;
|
||||
|
||||
public TraceCtrlPanel(RangeView view) {
|
||||
rangeView = view;
|
||||
setLayout(new BoxLayout(this, BoxLayout.X_AXIS));
|
||||
|
||||
traceButton = new JButton("Trace");
|
||||
traceButton.addActionListener(this);
|
||||
traceButton.setActionCommand("trace");
|
||||
traceButton.setToolTipText("Trace path of the end-effector");
|
||||
add(traceButton);
|
||||
}
|
||||
public void actionPerformed(ActionEvent e) {
|
||||
String s = e.getActionCommand();
|
||||
switch (s) {
|
||||
case "trace":
|
||||
rangeView.pathTraceEnabled = !rangeView.pathTraceEnabled;
|
||||
if (rangeView.pathTraceEnabled) {
|
||||
rangeView.pathTrace.clear();
|
||||
traceButton.setForeground(Color.GREEN);
|
||||
} else {
|
||||
traceButton.setForeground(Color.BLACK);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
System.out.println("Unknown Action Command:" + s);
|
||||
break;
|
||||
}
|
||||
}
|
||||
} // class TraceCtrlPanel
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
class ZoomCtrlPanel extends JPanel implements ActionListener {
|
||||
private RangeView rangeView;
|
||||
|
||||
public ZoomCtrlPanel(RangeView view) {
|
||||
rangeView = view;
|
||||
setBorder( BorderFactory.createEtchedBorder(EtchedBorder.RAISED));
|
||||
setLayout(new BoxLayout(this, BoxLayout.Y_AXIS));
|
||||
JButton zoomInButton = new JButton("+");
|
||||
zoomInButton.addActionListener(this);
|
||||
zoomInButton.setActionCommand("zoomin");
|
||||
zoomInButton.setToolTipText("Zoom In");
|
||||
add(zoomInButton);
|
||||
JButton zoomOutButton = new JButton("-");
|
||||
zoomOutButton.addActionListener(this);
|
||||
zoomOutButton.setActionCommand("zoomout");
|
||||
zoomOutButton.setToolTipText("Zoom Out");
|
||||
add(zoomOutButton);
|
||||
}
|
||||
public void actionPerformed(ActionEvent e) {
|
||||
String s = e.getActionCommand();
|
||||
switch (s) {
|
||||
case "zoomout":
|
||||
rangeView.setScale( rangeView.getScale() / 2 );
|
||||
break;
|
||||
case "zoomin":
|
||||
rangeView.setScale( rangeView.getScale() * 2 );
|
||||
break;
|
||||
default:
|
||||
System.out.println("Unknown Action Command:" + s);
|
||||
break;
|
||||
}
|
||||
}
|
||||
} // class ZoomCtrlPanel
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
class ControlPanel extends JPanel {
|
||||
|
||||
private RangeView rangeView;
|
||||
|
||||
public ModeCtrlPanel modeCtrlPanel;
|
||||
public JointCtrlPanel jointCtrlPanel;
|
||||
public VelocityCtrlPanel velocityCtrlPanel;
|
||||
public TraceCtrlPanel traceCtrlPanel;
|
||||
public ZoomCtrlPanel zoomCtrlPanel;
|
||||
|
||||
public ControlPanel(RangeView view) {
|
||||
|
||||
rangeView = view;
|
||||
setLayout(new BoxLayout(this, BoxLayout.X_AXIS));
|
||||
// ---------------------------------------------------------------------
|
||||
JLabel zoomControlLabel = new JLabel("Zoom");
|
||||
zoomControlLabel.setAlignmentX(Component.CENTER_ALIGNMENT);
|
||||
zoomCtrlPanel = new ZoomCtrlPanel(rangeView);
|
||||
zoomCtrlPanel.setAlignmentX(Component.CENTER_ALIGNMENT);
|
||||
JPanel labeledZoomCtrlPanel = new JPanel();
|
||||
labeledZoomCtrlPanel.setLayout(
|
||||
new BoxLayout(labeledZoomCtrlPanel, BoxLayout.Y_AXIS));
|
||||
labeledZoomCtrlPanel.add(zoomControlLabel);
|
||||
labeledZoomCtrlPanel.add( zoomCtrlPanel );
|
||||
add(labeledZoomCtrlPanel);
|
||||
// ---------------------------------------------------------------------
|
||||
traceCtrlPanel = new TraceCtrlPanel(rangeView);
|
||||
add(traceCtrlPanel);
|
||||
// ---------------------------------------------------------------------
|
||||
modeCtrlPanel = new ModeCtrlPanel();
|
||||
add(modeCtrlPanel);
|
||||
// ---------------------------------------------------------------------
|
||||
JLabel velCtrlLabelTop = new JLabel("EE Velocity");
|
||||
velCtrlLabelTop.setAlignmentX(Component.CENTER_ALIGNMENT);
|
||||
|
||||
JLabel velCtrlLabelBottom = new JLabel("[0..1] (m/s)");
|
||||
velCtrlLabelBottom.setAlignmentX(Component.CENTER_ALIGNMENT);
|
||||
|
||||
velocityCtrlPanel = new VelocityCtrlPanel();
|
||||
add(velocityCtrlPanel);
|
||||
|
||||
JPanel labeledVelCtrlPanel = new JPanel();
|
||||
labeledVelCtrlPanel.setLayout(
|
||||
new BoxLayout(labeledVelCtrlPanel, BoxLayout.Y_AXIS));
|
||||
|
||||
labeledVelCtrlPanel.add(velCtrlLabelTop);
|
||||
labeledVelCtrlPanel.add(velocityCtrlPanel);
|
||||
labeledVelCtrlPanel.add(velCtrlLabelBottom);
|
||||
|
||||
add(labeledVelCtrlPanel);
|
||||
// ---------------------------------------------------------------------
|
||||
|
||||
jointCtrlPanel = new JointCtrlPanel(rangeView.nlinks);
|
||||
add(jointCtrlPanel);
|
||||
}
|
||||
} // class ControlPanel
|
||||
//------------------------------------------------------------------------------
|
||||
public class RobotDisplay extends JFrame {
|
||||
|
||||
private RangeView rangeView;
|
||||
private BufferedReader in;
|
||||
private DataOutputStream out;
|
||||
private JPanel mainPanel;
|
||||
private JPanel viewPanel;
|
||||
private ControlPanel ctrlPanel;
|
||||
|
||||
public RobotDisplay() {
|
||||
rangeView = null;
|
||||
setTitle("Robot Range");
|
||||
setLocationRelativeTo(null);
|
||||
setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
|
||||
setFocusable(true);
|
||||
}
|
||||
|
||||
public void connectToServer(String host, int port ) throws IOException {
|
||||
Socket socket = new Socket(host, port);
|
||||
in = new BufferedReader(
|
||||
new InputStreamReader( socket.getInputStream()));
|
||||
out = new DataOutputStream(
|
||||
new BufferedOutputStream(socket.getOutputStream()));
|
||||
}
|
||||
|
||||
public void createGUI( int mapScale, int numberOfLinks ) {
|
||||
|
||||
setTitle("Robot Range");
|
||||
rangeView = new RangeView(mapScale, numberOfLinks);
|
||||
|
||||
JPanel viewPanel = new JPanel();
|
||||
viewPanel.setLayout(new BoxLayout(viewPanel, BoxLayout.X_AXIS));
|
||||
viewPanel.setBorder( BorderFactory.createEtchedBorder(EtchedBorder.RAISED));
|
||||
viewPanel.add(rangeView);
|
||||
|
||||
ctrlPanel = new ControlPanel(rangeView);
|
||||
|
||||
JPanel mainPanel = new JPanel();
|
||||
mainPanel.setLayout(new BoxLayout(mainPanel, BoxLayout.Y_AXIS));
|
||||
mainPanel.setBorder( BorderFactory.createEtchedBorder(EtchedBorder.RAISED));
|
||||
mainPanel.add(viewPanel);
|
||||
mainPanel.add(ctrlPanel);
|
||||
viewPanel.setPreferredSize(new Dimension(800, 650));
|
||||
ctrlPanel.setPreferredSize(new Dimension(800, 150));
|
||||
|
||||
add(mainPanel);
|
||||
pack();
|
||||
setVisible(true);
|
||||
}
|
||||
|
||||
public void drawRangeView() {
|
||||
rangeView.repaint();
|
||||
}
|
||||
|
||||
private static void printHelpText() {
|
||||
System.out.println(
|
||||
"----------------------------------------------------------------\n"
|
||||
+ "usage: java jar RobotDisplay.jar <port-number>\n"
|
||||
+ "----------------------------------------------------------------\n"
|
||||
);
|
||||
}
|
||||
|
||||
public enum ModelState { INACTIVE, READY, ACTIVE }
|
||||
|
||||
public static void main(String[] args)
|
||||
throws IOException, InterruptedException {
|
||||
|
||||
String host = "localHost";
|
||||
int port = 0;
|
||||
|
||||
// ---------------------------------------------------------------------
|
||||
// Handle program arguments.
|
||||
int ii = 0;
|
||||
while (ii < args.length) {
|
||||
switch (args[ii]) {
|
||||
case "-help" :
|
||||
case "--help" : {
|
||||
printHelpText();
|
||||
System.exit(0);
|
||||
} break;
|
||||
default : {
|
||||
port = (Integer.parseInt(args[ii]));
|
||||
} break;
|
||||
}
|
||||
++ii;
|
||||
}
|
||||
if (port == 0) {
|
||||
System.out.println("No variable server port specified.");
|
||||
printHelpText();
|
||||
System.exit(0);
|
||||
}
|
||||
|
||||
RobotDisplay robotDisplay = new RobotDisplay();
|
||||
|
||||
// Connect to the Trick simulation's variable server
|
||||
System.out.println("Connecting to: " + host + ":" + port);
|
||||
robotDisplay.connectToServer(host, port);
|
||||
robotDisplay.out.writeBytes(
|
||||
"trick.var_set_client_tag(\"RobotDisplay\") \n");
|
||||
robotDisplay.out.flush();
|
||||
|
||||
boolean go = true;
|
||||
|
||||
// Have the Variable Server send us the number of links ONCE.
|
||||
int nlinks = 0;
|
||||
robotDisplay.out.writeBytes(
|
||||
"trick.var_add(\"Manip2D.robot.kinemat.ndof\")\n" +
|
||||
"trick.var_send() \n" +
|
||||
"trick.var_clear() \n");
|
||||
robotDisplay.out.flush();
|
||||
try {
|
||||
String line;
|
||||
String field[];
|
||||
line = robotDisplay.in.readLine();
|
||||
field = line.split("\t");
|
||||
nlinks = Integer.parseInt( field[1]);
|
||||
} catch (IOException | NullPointerException e ) {
|
||||
go = false;
|
||||
}
|
||||
|
||||
int mapScale = 64 ; // pixels per meter.
|
||||
robotDisplay.createGUI(mapScale, nlinks);
|
||||
|
||||
// Configure the Variable Server to CYCLICALLY send data.
|
||||
robotDisplay.out.writeBytes(
|
||||
"trick.var_pause() \n");
|
||||
robotDisplay.out.writeBytes(
|
||||
"trick.var_add(\"Manip2D.robot.mode\")\n");
|
||||
robotDisplay.out.writeBytes(
|
||||
"trick.var_add(\"Manip2D.robot.controller.singleJointNum\")\n");
|
||||
for ( ii = 0; ii < nlinks; ii ++) {
|
||||
robotDisplay.out.writeBytes(
|
||||
String.format(
|
||||
"trick.var_add(\"Manip2D.robot.kinemat.joint_l[%d][0]\")\n",ii) +
|
||||
String.format(
|
||||
"trick.var_add(\"Manip2D.robot.kinemat.joint_q[%d]\")\n",ii)
|
||||
);
|
||||
}
|
||||
double cyclic_period = 0.050; // Seconds between variable server updates.
|
||||
robotDisplay.out.writeBytes(
|
||||
"trick.var_ascii() \n" +
|
||||
String.format("trick.var_cycle(%.3f)\n", cyclic_period) +
|
||||
"trick.var_unpause() \n" );
|
||||
robotDisplay.out.flush();
|
||||
|
||||
// Update the scene.
|
||||
robotDisplay.drawRangeView();
|
||||
|
||||
int robotMode = RobotCtrlMode.NOPE;
|
||||
int desiredMode;
|
||||
int jointNumber = 0;
|
||||
int desiredJointNumber;
|
||||
double desiredJointRate;
|
||||
while (go) {
|
||||
// -----------------------------------------------------------------
|
||||
// Recieve data from the variable server.
|
||||
try {
|
||||
String line;
|
||||
String field[];
|
||||
line = robotDisplay.in.readLine();
|
||||
field = line.split("\t");
|
||||
robotMode = Integer.parseInt( field[1]);
|
||||
robotDisplay.ctrlPanel.modeCtrlPanel.setMode( robotMode );
|
||||
jointNumber = Integer.parseInt( field[2]);
|
||||
robotDisplay.ctrlPanel.jointCtrlPanel.jointSelectPanel.setValue( jointNumber);
|
||||
robotDisplay.rangeView.setSelectedJoint(jointNumber);
|
||||
for ( int lix=0; lix < nlinks; lix++) {
|
||||
robotDisplay.rangeView.links[lix].l =
|
||||
Double.parseDouble( field[lix*2 + 3]);
|
||||
robotDisplay.rangeView.links[lix].q =
|
||||
Double.parseDouble( field[lix*2 + 4]);
|
||||
}
|
||||
} catch (IOException | NullPointerException e ) {
|
||||
go = false;
|
||||
}
|
||||
// -----------------------------------------------------------------
|
||||
// Send data to the variable server.
|
||||
desiredMode = robotDisplay.ctrlPanel.modeCtrlPanel.desiredMode();
|
||||
if ( desiredMode != RobotCtrlMode.NOPE ) {
|
||||
robotDisplay.ctrlPanel.modeCtrlPanel.resetDesiredMode();
|
||||
robotDisplay.out.writeBytes(String.format(
|
||||
"Manip2D.robot.mode = " + desiredMode + ";\n" ));
|
||||
robotDisplay.out.flush();
|
||||
}
|
||||
// Data for RobotCtrlMode.MANUAL mode.
|
||||
float Vx = robotDisplay.ctrlPanel.velocityCtrlPanel.getXvalue();
|
||||
robotDisplay.out.writeBytes(String.format
|
||||
("Manip2D.robot.controller.manualCommandRate[0] = " + Vx + ";\n" ));
|
||||
float Vy = robotDisplay.ctrlPanel.velocityCtrlPanel.getYvalue();
|
||||
robotDisplay.out.writeBytes(String.format
|
||||
("Manip2D.robot.controller.manualCommandRate[1] = " + Vy + ";\n" ));
|
||||
|
||||
// Data for RobotCtrlMode.SINGLE_JOINT mode.
|
||||
desiredJointNumber =
|
||||
robotDisplay.ctrlPanel.jointCtrlPanel.jointSelectPanel.getValue();
|
||||
if (desiredJointNumber != jointNumber) {
|
||||
robotDisplay.out.writeBytes(String.format
|
||||
("Manip2D.robot.controller.singleJointNum = " +
|
||||
desiredJointNumber + ";\n" ));
|
||||
}
|
||||
|
||||
desiredJointRate =
|
||||
robotDisplay.ctrlPanel.jointCtrlPanel.jointRatePanel.getValue();
|
||||
|
||||
robotDisplay.out.writeBytes(String.format
|
||||
("Manip2D.robot.controller.singleJointRate = " +
|
||||
desiredJointRate + ";\n" ));
|
||||
|
||||
// Data for RobotCtrlMode.EEPOS mode.
|
||||
robotDisplay.out.writeBytes(String.format
|
||||
("Manip2D.robot.controller.desiredPos[0] = " +
|
||||
robotDisplay.rangeView.desiredPosition.x + ";\n" ));
|
||||
robotDisplay.out.writeBytes(String.format
|
||||
("Manip2D.robot.controller.desiredPos[1] = " +
|
||||
robotDisplay.rangeView.desiredPosition.y + ";\n" ));
|
||||
|
||||
// -----------------------------------------------------------------
|
||||
// Update the scene.
|
||||
robotDisplay.drawRangeView();
|
||||
} // while
|
||||
} // main
|
||||
} // class
|
||||
264
trick_sims/SIM_robot/models/kinematics/ManipKinemat.cc
Normal file
264
trick_sims/SIM_robot/models/kinematics/ManipKinemat.cc
Normal file
@ -0,0 +1,264 @@
|
||||
/********************************* TRICK HEADER *******************************
|
||||
PURPOSE: ( Simulate a planar, 2DOF manipulator for Trick example sim.)
|
||||
LIBRARY DEPENDENCY:
|
||||
((kinematics/ManipKinemat.o)
|
||||
(utils/utils.o)
|
||||
)
|
||||
PROGRAMMERS:
|
||||
(((Sean Harmeyer) (NASA) (Dec 2022) (Trick Example Sim)))
|
||||
*******************************************************************************/
|
||||
|
||||
#include "kinematics/ManipKinemat.hh"
|
||||
#include "include/trick/memorymanager_c_intf.h"
|
||||
|
||||
ManipKinemat::ManipKinemat(int numDof):ndof(numDof)
|
||||
{
|
||||
|
||||
int i,j;
|
||||
|
||||
/* These vectors and matrices don't explicitly depend on the
|
||||
* number of degrees of freedom */
|
||||
V_ee = (double *)TMM_declare_var_1d("double",2);
|
||||
P_task_base = (double *)TMM_declare_var_1d("double",2);
|
||||
P_joint_ee = (double *)TMM_declare_var_1d("double",2);
|
||||
P_task_ee = (double *)TMM_declare_var_1d("double",2);
|
||||
joint_ee = (double *)TMM_declare_var_1d("double",2);
|
||||
|
||||
R_task_base = (double **)TMM_declare_var_1d("double*",2);
|
||||
R_base_task = (double **)TMM_declare_var_1d("double*",2);
|
||||
R_ee_joint = (double **)TMM_declare_var_1d("double*",2);
|
||||
jacobian = (double **)TMM_declare_var_1d("double*",2);
|
||||
R_ee_task = (double **)TMM_declare_var_1d("double*",2);
|
||||
|
||||
/* total number of degrees of freedom determines the size of all of these
|
||||
* vectors and matrices, so we size them accordingly */
|
||||
|
||||
joint_u = (double *)TMM_declare_var_1d("double",ndof);
|
||||
joint_q = (double *)TMM_declare_var_1d("double",ndof);
|
||||
joint_w = (double *)TMM_declare_var_1d("double",ndof);
|
||||
|
||||
joint_l = (double **)TMM_declare_var_1d("double*",ndof);
|
||||
jacInv = (double **)TMM_declare_var_1d("double*",ndof);
|
||||
|
||||
P_joint_joint = (double **)TMM_declare_var_1d("double*",ndof);
|
||||
P_task_joint = (double **)TMM_declare_var_1d("double*",ndof);
|
||||
V_joint_joint = (double **)TMM_declare_var_1d("double*",ndof);
|
||||
V_joint_task = (double **)TMM_declare_var_1d("double*",ndof);
|
||||
|
||||
R_joint = (double ***)TMM_declare_var_1d("double**",ndof);
|
||||
R_joint_task = (double ***)TMM_declare_var_1d("double**",ndof);
|
||||
|
||||
for(i=0;i<2;i++)
|
||||
{
|
||||
R_task_base[i] = (double *)TMM_declare_var_1d("double",2);
|
||||
R_base_task[i] = (double *)TMM_declare_var_1d("double",2);
|
||||
R_ee_task[i] = (double *)TMM_declare_var_1d("double",2);
|
||||
R_ee_joint[i] = (double *)TMM_declare_var_1d("double",2);
|
||||
jacobian[i] = (double *)TMM_declare_var_1d("double",ndof);
|
||||
}
|
||||
|
||||
|
||||
for(i=0;i<ndof;i++)
|
||||
{
|
||||
|
||||
joint_l[i] = (double *)TMM_declare_var_1d("double",2);
|
||||
P_joint_joint[i] = (double *)TMM_declare_var_1d("double",2);
|
||||
P_task_joint[i] = (double *)TMM_declare_var_1d("double",2);
|
||||
V_joint_joint[i] = (double *)TMM_declare_var_1d("double",2);
|
||||
V_joint_task[i] = (double *)TMM_declare_var_1d("double",2);
|
||||
|
||||
R_joint[i] = (double **)TMM_declare_var_1d("double*",2);
|
||||
R_joint_task[i] = (double **)TMM_declare_var_1d("double*",2);
|
||||
jacInv[i] = (double *)TMM_declare_var_1d("double",2);
|
||||
|
||||
for(j=0;j<ndof;j++)
|
||||
{
|
||||
R_joint[i][j] = (double *)TMM_declare_var_1d("double",2);
|
||||
R_joint_task[i][j] = (double *)TMM_declare_var_1d("double",2);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* base relative to task in task frame*/
|
||||
P_task_base[0] = 0;
|
||||
P_task_base[1] = 0;
|
||||
|
||||
R_base_task[0][0] = 1;
|
||||
R_base_task[0][1] = 0;
|
||||
R_base_task[1][0] = 0;
|
||||
R_base_task[1][1] = 1;
|
||||
|
||||
R_task_base[0][0] = 1;
|
||||
R_task_base[0][1] = 0;
|
||||
R_task_base[1][0] = 0;
|
||||
R_task_base[1][1] = 1;
|
||||
|
||||
|
||||
/* initial joint angles */
|
||||
joint_q[0] = 0;
|
||||
joint_q[1] = 0;
|
||||
|
||||
/* link lengths */
|
||||
joint_l[0][0] = 2;
|
||||
joint_l[0][1] = 0;
|
||||
joint_l[1][0] = 2;
|
||||
joint_l[1][1] = 0;
|
||||
|
||||
/* EE offset */
|
||||
joint_ee[0] = 0;
|
||||
joint_ee[1] = 0;
|
||||
|
||||
/* Rotation from EE to previous joint */
|
||||
R_ee_joint[0][0] = 1;
|
||||
R_ee_joint[0][1] = 0;
|
||||
R_ee_joint[1][0] = 0;
|
||||
R_ee_joint[1][1] = 1;
|
||||
|
||||
jacobian[0][0] = 0;
|
||||
jacobian[0][1] = 0;
|
||||
jacobian[1][0] = 0;
|
||||
jacobian[1][1] = 0;
|
||||
|
||||
jacDet = 0;
|
||||
|
||||
singularityTolerance = .17453;
|
||||
checkSingularities = true;
|
||||
|
||||
}
|
||||
|
||||
void ManipKinemat::forwardKinPos()
|
||||
{
|
||||
int i; // loop counter
|
||||
double c,s; // sine and cosine shorthand
|
||||
|
||||
// rescale angles to 0 < q < 2*PI
|
||||
for(i=0; i<ndof; i++)
|
||||
{
|
||||
if (joint_q[i] > 2.0 * M_PI)
|
||||
joint_q[i] -= 2.0 * M_PI;
|
||||
else if (joint_q[i] < -2.0 * M_PI)
|
||||
joint_q[i] += 2.0 * M_PI;
|
||||
}
|
||||
|
||||
// Calculate current rotation matrices for all frames
|
||||
for (i=0; i<ndof; i++)
|
||||
{
|
||||
c = cos(joint_q[i]);
|
||||
s = sin(joint_q[i]);
|
||||
|
||||
R_joint[i][0][0]= c;
|
||||
R_joint[i][0][1]= -s;
|
||||
R_joint[i][1][0]= s;
|
||||
R_joint[i][1][1]= c;
|
||||
|
||||
// Calculate position of every joint tip in previous frame
|
||||
utils.MVMult(P_joint_joint[i], (double** )R_joint[i], joint_l[i]);
|
||||
}
|
||||
|
||||
|
||||
// Calculate R from every joint back to task
|
||||
utils.Transpose(R_base_task, R_task_base);
|
||||
|
||||
for(i=0;i<ndof;i++)
|
||||
{
|
||||
if(i == 0)
|
||||
utils.MMMult(R_joint_task[i], R_base_task, R_joint[i]);
|
||||
else
|
||||
utils.MMMult(R_joint_task[i], R_joint_task[i-1], R_joint[i]);
|
||||
}
|
||||
|
||||
// Calculate R from EE to task
|
||||
utils.MMMult(R_ee_task, R_joint_task[ndof-1], R_ee_joint);
|
||||
|
||||
|
||||
|
||||
// Calculate vector from task to each joint in task
|
||||
for(i=0;i<ndof;i++)
|
||||
{
|
||||
if (i==0)
|
||||
utils.MVMult(P_task_joint[i],R_joint_task[i],joint_l[i]);
|
||||
else
|
||||
{
|
||||
utils.MVMult(P_task_joint[i],R_joint_task[i],joint_l[i]);
|
||||
utils.VAdd(P_task_joint[i],P_task_joint[i],P_task_joint[i-1]);
|
||||
}
|
||||
}
|
||||
|
||||
// Calculate vector from task to EE in task
|
||||
|
||||
utils.MVMult(P_task_ee,R_ee_task,joint_ee);
|
||||
utils.VAdd(P_task_ee,P_task_ee,P_task_joint[ndof-1]);
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void ManipKinemat::forwardKinVel()
|
||||
{
|
||||
|
||||
/* like the jacobian, these are hand-calculated for a 2-dof planar.
|
||||
* Anything more would require velocity propagation to determine the
|
||||
* velocity of the end-effector and intermediate points */
|
||||
|
||||
double l1, l2, s1, c1, s12, c12;
|
||||
l1 = joint_l[0][0];
|
||||
l2 = joint_l[1][0];
|
||||
|
||||
s1 = sin(joint_q[0]);
|
||||
s12 = sin(joint_q[0]+joint_q[1]);
|
||||
|
||||
c1 = cos(joint_q[0]);
|
||||
c12 = cos(joint_q[0]+joint_q[1]);
|
||||
|
||||
|
||||
V_ee[0] = ( -l1*s1 - l2*s12 )*joint_w[0] - l2*s12*joint_w[1];
|
||||
V_ee[1] = ( l1*c1 + l2*c12 )*joint_w[0] + l2*c12*joint_w[1];
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
void ManipKinemat::calcJacobian()
|
||||
{
|
||||
|
||||
/* These are hand-calculated due to the simplicity of the manipulator
|
||||
* system. The algorithmic method of determining the elements of
|
||||
* the jacobian, while efficient and generic, are difficult to read in
|
||||
* code and are beyond the scope of a simple robotic tutorial
|
||||
*/
|
||||
|
||||
double s1,c1,s12,c12;
|
||||
s1 = sin(joint_q[0]);
|
||||
s12 = sin(joint_q[0]+joint_q[1]);
|
||||
c1 = cos(joint_q[0]);
|
||||
c12 = cos(joint_q[0]+joint_q[1]);
|
||||
|
||||
double l1, l2;
|
||||
l1 = joint_l[0][0];
|
||||
l2 = joint_l[1][0];
|
||||
|
||||
jacobian[0][0] = -l1*s1 - l2*s12;
|
||||
jacobian[0][1] = -l2*s12;
|
||||
jacobian[1][0] = l1*c1 + l2*c12;
|
||||
jacobian[1][1] = l2*c12;
|
||||
|
||||
|
||||
/* Calculate the inverse jacobian as well */
|
||||
|
||||
jacDet = jacobian[0][0]*jacobian[1][1] - jacobian[0][1]*jacobian[1][0];
|
||||
|
||||
jacInv[0][0] = jacobian[1][1]/jacDet;
|
||||
jacInv[0][1] = -jacobian[0][1]/jacDet;
|
||||
jacInv[1][0] = -jacobian[1][0]/jacDet;
|
||||
jacInv[1][1] = jacobian[0][0]/jacDet;
|
||||
|
||||
}
|
||||
|
||||
bool ManipKinemat::isSingular()
|
||||
{
|
||||
if( (fabs(joint_q[1]) < singularityTolerance) || ( fabs( fabs(joint_q[1]) - M_PI) < singularityTolerance ) )
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
72
trick_sims/SIM_robot/models/kinematics/ManipKinemat.hh
Normal file
72
trick_sims/SIM_robot/models/kinematics/ManipKinemat.hh
Normal file
@ -0,0 +1,72 @@
|
||||
#ifndef __MANIPKINEMAT_HH_
|
||||
#define __MANIPKINEMAT_HH_
|
||||
/**************************************************************************
|
||||
PURPOSE: (2D Manipulator kinematics)
|
||||
LIBRARY DEPENDENCY: ( (kinematics/ManipKinemat.cc) )
|
||||
***************************************************************************/
|
||||
#include <cmath>
|
||||
#include "include/trick/exec_proto.h"
|
||||
#include "utils/utils.hh"
|
||||
#include <iostream>
|
||||
class ManipKinemat
|
||||
{
|
||||
public:
|
||||
|
||||
|
||||
double *joint_u; /* -- axis of rotation for each joint */
|
||||
double *joint_q; /* rad joint angle of each joint */
|
||||
double *joint_w; /* rad/s joint rate of each joint */
|
||||
|
||||
double **joint_l; /* m link lengths of each joint */
|
||||
double *joint_ee; /* m length from last joint to ee */
|
||||
|
||||
|
||||
double *P_task_base; /* m vector task->base expressed in task */
|
||||
double **R_task_base; /* -- rotation matrix task->base */
|
||||
double **R_base_task; /* -- rotation matrix base->task */
|
||||
|
||||
//(first index is base->joint_0 expressed in base)
|
||||
double **P_joint_joint; /* m link_i+1->link_i in frame i */
|
||||
|
||||
double *P_joint_ee; /* m link_n-1 -> ee in frame n-1 */
|
||||
double **P_task_joint; /* m task->link_i in task frame */
|
||||
double *P_task_ee; /* m task->ee in task frame */
|
||||
|
||||
|
||||
|
||||
// first index is joint_0->base rotation
|
||||
double ***R_joint; /* -- rotation matrix frames i+1->i */
|
||||
|
||||
double ***R_joint_task; /* -- rotation matrix frame i+1-task */
|
||||
double **R_ee_joint; /* -- rotation matrix frame ee->n */
|
||||
|
||||
double **R_ee_task; /* -- rotation matrix frame ee->task */
|
||||
|
||||
|
||||
double **V_joint_joint; /* m/s velocity of frame i in i-1 */
|
||||
double *V_ee; /* m/s vel of EE in task */
|
||||
double **V_joint_task; /* m/s vel of joint i in task frame */
|
||||
|
||||
double **jacobian; /* -- transform from angular to linear vel */
|
||||
double **jacInv; /* -- Inverse Jacobian */
|
||||
double jacDet; /* -- Determinant of the Jacobian */
|
||||
|
||||
|
||||
bool checkSingularities; /* -- whether to check for kinematic singularities */
|
||||
double singularityTolerance; /* rad tolerance for singularity approach */
|
||||
|
||||
ManipUtils utils;
|
||||
|
||||
ManipKinemat(int numDof);
|
||||
|
||||
void forwardKinPos();
|
||||
void forwardKinVel();
|
||||
void calcJacobian();
|
||||
bool isSingular();
|
||||
|
||||
private:
|
||||
|
||||
int ndof; /* -- number of degrees of freedom */
|
||||
};
|
||||
|
||||
#endif
|
||||
138
trick_sims/SIM_robot/models/manipulator/manipulator.cc
Normal file
138
trick_sims/SIM_robot/models/manipulator/manipulator.cc
Normal file
@ -0,0 +1,138 @@
|
||||
/********************************* TRICK HEADER *******************************
|
||||
PURPOSE: ( Simulate a planar, 2DOF manipulator for Trick example sim.)
|
||||
LIBRARY DEPENDENCY:
|
||||
(
|
||||
(manipulator/manipulator.o)
|
||||
(kinematics/ManipKinemat.o)
|
||||
(control/ManipControl.o)
|
||||
(utils/utils.o)
|
||||
)
|
||||
PROGRAMMERS:
|
||||
(((Sean Harmeyer) (NASA) (Dec 2022) (Trick Example Sim)))
|
||||
*******************************************************************************/
|
||||
|
||||
#include "manipulator/manipulator.hh"
|
||||
|
||||
PlanarManip::PlanarManip(int numDof) : ndof(numDof), kinemat(numDof), controller(numDof)
|
||||
{
|
||||
|
||||
int i = 0;
|
||||
/* just throw some pointers around */
|
||||
|
||||
controller.R_ee_task = kinemat.R_ee_task;
|
||||
controller.jacInv = kinemat.jacInv;
|
||||
for(i=0;i<ndof;i++)
|
||||
{
|
||||
controller.jacInv[i] = kinemat.jacInv[i];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void PlanarManip::calcKinematics()
|
||||
{
|
||||
|
||||
kinemat.forwardKinPos();
|
||||
kinemat.forwardKinVel();
|
||||
kinemat.calcJacobian();
|
||||
|
||||
}
|
||||
|
||||
void PlanarManip::control()
|
||||
{
|
||||
|
||||
bool positionReached = false;
|
||||
switch(mode)
|
||||
{
|
||||
/* No need to check singularities in SingleJoint mode, because they only arise with inverse
|
||||
* Jacobians and there's no need to use an inverse Jacobian here */
|
||||
case SingleJoint:
|
||||
|
||||
if (controller.singleJointNum < 0 || controller.singleJointNum > ndof)
|
||||
controller.singleJointNum = 0;
|
||||
|
||||
controller.singleJointControl();
|
||||
break;
|
||||
|
||||
case Manual:
|
||||
if ( kinemat.checkSingularities && kinemat.isSingular() )
|
||||
{
|
||||
std::cout<<"Singularity approached, EE control disabled."<<std::endl;
|
||||
mode = Nope;
|
||||
controller.clearControlCommands();
|
||||
return;
|
||||
}
|
||||
|
||||
controller.manualControl();
|
||||
break;
|
||||
|
||||
case EEPos:
|
||||
if ( kinemat.checkSingularities )
|
||||
{
|
||||
if ( !kinemat.isSingular() )
|
||||
positionReached = controller.EEPositionAuto( &(kinemat.P_task_ee[0]), &(kinemat.V_ee[0]) );
|
||||
else
|
||||
{
|
||||
std::cout<<"Singularity approached, EE control disabled."<<std::endl;
|
||||
mode = Nope;
|
||||
controller.clearControlCommands();
|
||||
return;
|
||||
}
|
||||
}
|
||||
else
|
||||
positionReached = controller.EEPositionAuto( &(kinemat.P_task_ee[0]), &(kinemat.V_ee[0]) );
|
||||
|
||||
break;
|
||||
|
||||
case Cancel:
|
||||
controller.clearControlCommands();
|
||||
mode = Nope;
|
||||
break;
|
||||
default:
|
||||
case Nope:
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int PlanarManip::stateDeriv()
|
||||
{
|
||||
return(0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
int PlanarManip::updateState()
|
||||
{
|
||||
|
||||
int integration_step, i;
|
||||
|
||||
for(i=0;i<ndof;i++)
|
||||
{
|
||||
/* This is a kinematic sim with perfect control, so the commanded joint rates
|
||||
* magically become the real joint rates */
|
||||
kinemat.joint_w[i] = controller.commandedJointRate[i];
|
||||
}
|
||||
|
||||
for(i = 0; i < ndof; i++)
|
||||
load_indexed_state(i, kinemat.joint_q[i]);
|
||||
for(i=0;i<ndof;i++)
|
||||
load_indexed_deriv(i, kinemat.joint_w[i]);
|
||||
|
||||
integration_step = integrate();
|
||||
|
||||
for(i=0;i<ndof;i++)
|
||||
{
|
||||
kinemat.joint_q[i] = unload_indexed_state(i);
|
||||
kinemat.joint_q[i] = fmod(kinemat.joint_q[i], 2*M_PI);
|
||||
}
|
||||
|
||||
|
||||
// std::cout << "q = [ " << kinemat.joint_q[0] << kinemat.joint_q[1] << " ]" << std::endl;
|
||||
|
||||
return(integration_step);
|
||||
|
||||
}
|
||||
|
||||
|
||||
45
trick_sims/SIM_robot/models/manipulator/manipulator.hh
Normal file
45
trick_sims/SIM_robot/models/manipulator/manipulator.hh
Normal file
@ -0,0 +1,45 @@
|
||||
#ifndef __MANIPULATOR_HH_
|
||||
#define __MANIPULATOR_HH_
|
||||
/**************************************************************************
|
||||
PURPOSE: (2D Manipulator class definitions including kinematics and control)
|
||||
***************************************************************************/
|
||||
|
||||
#include "kinematics/ManipKinemat.hh"
|
||||
#include "control/ManipControl.hh"
|
||||
#include "utils/utils.hh"
|
||||
#include "trick/integrator_c_intf.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <new>
|
||||
|
||||
enum ControlMode
|
||||
{
|
||||
Nope, /* No control, just chill */
|
||||
Cancel, /* Cancel current mode, clear control vars, and set to Nope */
|
||||
SingleJoint, /* Control a single joint drive direction */
|
||||
Manual, /* Inv Kin resolved-rate control of end effector rate */
|
||||
EEPos /* Drive EE to desired location */
|
||||
};
|
||||
|
||||
class PlanarManip
|
||||
{
|
||||
public:
|
||||
|
||||
int ndof; /* -- number of degrees of freedom */
|
||||
ControlMode mode; /* -- Control mode to use */
|
||||
|
||||
ManipKinemat kinemat; /* -- class the calculates for/inv kinematics */
|
||||
ManipControl controller; /* -- controller class */
|
||||
|
||||
PlanarManip(int numDof);
|
||||
|
||||
void calcKinematics(); /* -- call kinematics routines */
|
||||
void control(); /* -- call control routines */
|
||||
int stateDeriv(); /* -- update velocities for integration */
|
||||
int updateState(); /* -- update manipulator state */
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
68
trick_sims/SIM_robot/models/utils/utils.cc
Normal file
68
trick_sims/SIM_robot/models/utils/utils.cc
Normal file
@ -0,0 +1,68 @@
|
||||
/********************************* TRICK HEADER *******************************
|
||||
PURPOSE: ( Linear algebra routines for 2x2 systems)
|
||||
LIBRARY DEPENDENCY:
|
||||
((utils/utils.o))
|
||||
ASSUMPTIONS:
|
||||
( Square 2x2 matrices )
|
||||
PROGRAMMERS:
|
||||
(((Sean Harmeyer) (NASA) (Dec 2022) (Trick Example Sim)))
|
||||
*******************************************************************************/
|
||||
#include "utils/utils.hh"
|
||||
|
||||
void ManipUtils::VCross(double *c, double *a, double *b)
|
||||
{
|
||||
c[0] = a[1]*b[2] - b[1]*a[2];
|
||||
c[1] = -(a[0]*b[2] - b[0]*a[2]);
|
||||
c[2] = a[0]*b[1] - b[0]*a[1];
|
||||
}
|
||||
|
||||
|
||||
void ManipUtils::MMMult(double **c, double **a, double **b)
|
||||
{
|
||||
|
||||
double c11,c12,c21,c22;
|
||||
|
||||
c11 = a[0][0]*b[0][0] + a[0][1]*b[1][0];
|
||||
c12 = a[0][0]*b[0][1] + a[0][1]*b[1][1];
|
||||
c21 = a[1][0]*b[0][0] + a[1][1]*b[1][0];
|
||||
c22 = a[1][0]*b[0][1] + a[1][1]*b[1][1];
|
||||
|
||||
c[0][0] = c11;
|
||||
c[0][1] = c12;
|
||||
c[1][0] = c21;
|
||||
c[1][1] = c22;
|
||||
|
||||
}
|
||||
|
||||
void ManipUtils::MVMult(double *c, double **a, double *b)
|
||||
{
|
||||
|
||||
double c11,c12;
|
||||
|
||||
c11 = a[0][0]*b[0] + a[0][1]*b[1];
|
||||
c12 = a[1][0]*b[0] + a[1][1]*b[1];
|
||||
|
||||
c[0] = c11;
|
||||
c[1] = c12;
|
||||
|
||||
}
|
||||
|
||||
void ManipUtils::Transpose(double **trans, double **a)
|
||||
{
|
||||
int i,j;
|
||||
for(i=0;i<2;i++)
|
||||
{
|
||||
for(j=0;j<2;j++)
|
||||
{
|
||||
trans[i][j]=a[j][i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ManipUtils::VAdd(double *c, double *a, double *b)
|
||||
{
|
||||
int i;
|
||||
for(i=0;i<2;i++)
|
||||
c[i] = a[i] + b[i];
|
||||
}
|
||||
|
||||
23
trick_sims/SIM_robot/models/utils/utils.hh
Normal file
23
trick_sims/SIM_robot/models/utils/utils.hh
Normal file
@ -0,0 +1,23 @@
|
||||
#ifndef __MANIPUTILS_HH_
|
||||
#define __MANIPUTILS_HH_
|
||||
/**************************************************************************
|
||||
PURPOSE: (2x2 Linear Algebra utilities)
|
||||
***************************************************************************/
|
||||
|
||||
|
||||
|
||||
class ManipUtils
|
||||
{
|
||||
public:
|
||||
|
||||
void MMMult(double **c, double **a, double **b);
|
||||
void MVMult(double *c, double **a, double *b);
|
||||
void Transpose(double **aT, double **a);
|
||||
void VCross(double *c, double *a, double *b);
|
||||
void VAdd(double *c, double *a, double *b);
|
||||
|
||||
ManipUtils(){};
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue
Block a user