Embedded Files
MEC231A: Experiential Advanced Control Design I. Partners: Thena Guttieri, Julia Pecego, Aryaman Jhunjhunwala
MEC231A: Experiential Advanced Control Design I. Partners: Thena Guttieri, Julia Pecego, Aryaman Jhunjhunwala
Problem Statement: Can an MPC model provide accurate tracking for a robotic arm? In our previous courses, we've done PID control with the Sawyer arm and movement with orientation or path constraints. We would like to see if we can also control the Sawyer arm with an MPC model with enforcing these constraints.
Problem Statement: Can an MPC model provide accurate tracking for a robotic arm? In our previous courses, we've done PID control with the Sawyer arm and movement with orientation or path constraints. We would like to see if we can also control the Sawyer arm with an MPC model with enforcing these constraints.
Background:
Background:
Inverse Kinematics (IK)
Inverse Kinematics (IK)
The inverse kinematics of a robotic arm tells us the required joint angles for a given end effector pose (XYZ position and orientation). Given that there are 7 degrees of freedom on the Sawyer Robot, there are multiple solutions that each joint can be at given a pose. Typically we would use a IK solver that is available, such as a rosservice available through the Sawyer SDK.
Forward Kinematics (FK)
Forward Kinematics (FK)
In the other direction, the forward kinematics of a robotic arm can tell us the end-effector pose given the angles of each joint. This is computed by computing the twists of each joint in the zero configuration of the robot. We followed the algorithm on the right to calculate the inverse kinematics.
Source: UC Berkeley, EECS106A, Lab 3: Forward Kinematics
Approach #1: Forecast the Dynamics Model Using Endpoint Velocity
Approach #1: Forecast the Dynamics Model Using Endpoint Velocity
State: End effector XYZ Position
State: End effector XYZ Position
Input: Joint Velocities
Input: Joint Velocities
This approach uses the readily available endpoint pose and velocity through the Limb SDK of the Sawyer arm to use the end effector as a state. It would change by the end-effector velocity, and we would forecast the dynamics in our horizon by calculating the spatial manipulator Jacobian.
Source: Richard Murray, Zexiang Li and S. Shankar Sastry's A Mathematical Introduction to Robotic Manipulation
Limb SDK
Issues:
Issues:
However, when we recorded the Sawyer arm endpoint pose and computed our own forecast with the end effector velocity, the data did not match. We suspect that this may be because we are given both a linear and angular endpoint velocity, but we are unsure about the angular velocity's axis and used it incorrectly.
However, when we recorded the Sawyer arm endpoint pose and computed our own forecast with the end effector velocity, the data did not match. We suspect that this may be because we are given both a linear and angular endpoint velocity, but we are unsure about the angular velocity's axis and used it incorrectly.
Top Right Figure: Data vs. Forecast with Provided Endpoint Velocity
Bottom Right Figure: Data vs. Forecast with Calculated Spatial Jacobian
Approach #2: Generate our own end effector path and perform IK on each point
Approach #2: Generate our own end effector path and perform IK on each point
IK Output: A series of joint angles for our MPC to follow
IK Output: A series of joint angles for our MPC to follow
State: Joint Angles
State: Joint Angles
Input: Joint Velocities
Input: Joint Velocities
With this approach, we would define our own trajectory in 3D and use the inverse kinematics solver to computer the joint angles required for each point. The output is getting a series of reference joint angles for our MPC to follow.
Issues:
Issues:
Although this may be a simpler solution, we do not have control over what kind of joint angles the IK solver will pick. Since there are multiple solutions, there is a chance that the IK solution from one point to the next can be far away from each other, forcing the arm to move by abruptly at the next time step. The way around this is to call the solver multiple times and choose the IK solution closest to the previous configuration; however, the IK service we had access to only provided one solution at a time instead of all possible solutions. This meant that the solver had to be called enough times until it found the optimal solution, for which we do not have a guaranteed maximum of attempts that are needed. This won't allow the controller to be run in real time if we gave it multiple instantaneous goals.
Although this may be a simpler solution, we do not have control over what kind of joint angles the IK solver will pick. Since there are multiple solutions, there is a chance that the IK solution from one point to the next can be far away from each other, forcing the arm to move by abruptly at the next time step. The way around this is to call the solver multiple times and choose the IK solution closest to the previous configuration; however, the IK service we had access to only provided one solution at a time instead of all possible solutions. This meant that the solver had to be called enough times until it found the optimal solution, for which we do not have a guaranteed maximum of attempts that are needed. This won't allow the controller to be run in real time if we gave it multiple instantaneous goals.
We would also not be able to use the next state as an equality constraint because each state is decoupled. If we specify the next joint angle needed, then the joint velocity required given the sampling time is already unique.
We would also not be able to use the next state as an equality constraint because each state is decoupled. If we specify the next joint angle needed, then the joint velocity required given the sampling time is already unique.
Final Approach: Output Feedback MPC
Final Approach: Output Feedback MPC
State: Joint Angles
State: Joint Angles
Input: Joint Velocities
Input: Joint Velocities
Output: End effector position
Output: End effector position
With this approach, we use the same dynamic model for the joint states. However, our cost function will be in terms of the forward kinematics of the joint angles.
Model Verification
Model Verification
While recording the endpoint pose and joint angles of the robot, we confirmed that our forward kinematics map matched the given data and is a valid way to forecast over the horizon.
While recording the endpoint pose and joint angles of the robot, we confirmed that our forward kinematics map matched the given data and is a valid way to forecast over the horizon.
Constraints of the MPC Problem
Constraints of the MPC Problem
For input constraints, we choose an arbitrary value of +/- 0.2 m/s for lab safety.
For input constraints, we choose an arbitrary value of +/- 0.2 m/s for lab safety.
Joint Angle Restrictions
Joint Angle Restrictions
Enforcing self collision avoidance as a function of the joint angles is difficult as there may not be a convex region to confine them in. Although there may be a way to utilize built-in tools, the Pyomo variables were not able to be used in external libraties. In this case, we decided to move the joint angles ourselves until we saw a collision warning in MoveIt and defined our own set of joint states to ensure collisions would never happen.
MPC Trials: State + Input Constraints
MPC Trials: State + Input Constraints
Here, we have a trial with our state and input constraints. As can be seen, our desired position is fairly close to our actual position. Additionally, each joint position and joint velocity is mapped over time to see how the arm got to this orientation
Here, we have a trial with our state and input constraints. As can be seen, our desired position is fairly close to our actual position. Additionally, each joint position and joint velocity is mapped over time to see how the arm got to this orientation
MPC Trials: Orientation Constraints
MPC Trials: Orientation Constraints
Our robot gives us a quaternion that starts close to [0, -1, 0, 0]. This results in an end-effector, or in other words our gripper, that is pointed downwards. We found the resulting rotation matrix to match this quaternion as can be seen in the right. We then impose constraints on this rotation matrix because that is what we can control. Like with the state and input constraints, the actual position is close to the desired, however, it isn’t perfect.
Our robot gives us a quaternion that starts close to [0, -1, 0, 0]. This results in an end-effector, or in other words our gripper, that is pointed downwards. We found the resulting rotation matrix to match this quaternion as can be seen in the right. We then impose constraints on this rotation matrix because that is what we can control. Like with the state and input constraints, the actual position is close to the desired, however, it isn’t perfect.
Since our starting position may not exactly be within this constraint, we define a small epsilon region around each element.
Since our starting position may not exactly be within this constraint, we define a small epsilon region around each element.
MPC Trials: Path Constraints
MPC Trials: Path Constraints
Finally, our last trial is with path constraints. Here, we want our end-effector to follow a line or specified trajectory. In order to do this, we fix our y and z direction within a given tolerance and just move the x direction of the end-effector to reach our desired position. This trial was the most accurate with our desired goal and actual goal aligning right on top of each other.
Finally, our last trial is with path constraints. Here, we want our end-effector to follow a line or specified trajectory. In order to do this, we fix our y and z direction within a given tolerance and just move the x direction of the end-effector to reach our desired position. This trial was the most accurate with our desired goal and actual goal aligning right on top of each other.
Since our starting position may not exactly be within this constraint, we again define a small epsilon region around each element.
Since our starting position may not exactly be within this constraint, we again define a small epsilon region around each element.
Our robot gives us a quaternion that starts close to [0, -1, 0, 0]. This results in an end-effector, or in other words our gripper, that is pointed downwards. We found the resulting rotation matrix to match this quaternion as can be seen on the screen. We then impose constraints on this rotation matrix because that is what we can control. Like with the state and input constraints, the actual position is close to the desired, however, it isn’t perfect.
Challenges:
Challenges:
Model Verification: Finding a dynamic model that provides us with an accurate forecast was fairly difficult for this project. There was limited documentation for the Sawyer arm. Another method we saw in E. Guechi, S. Bouzoualegh, Y. Zennir, and S. Blazic MPC Control and LQ Optimal Control of A Two-Link Robot Arm: A Comparative Study is to derive the Lagrangian model of the Sawyer robot using its gravity, inertial, and Coriolis matrices. This is another method we would like to try if given more time.
Joint Angle Restrictions: Since we experimentally found joint angles that would prevent self-collisions ourselves, this made them very restrictive and is not representative of all possible joint angles.
Forward Kinematics: Since numpy cannot operate on Pyomo variables, we had to rewrite much of the standard matrix-vector operation ourselves using list comprehensions.
Improvements & Extensions:
Improvements & Extensions:
Although we saw that the constrained finite-time optimal control (CFTOC) problem was able to be solved, it took longer as we added more constraints, with the longest being 3.5 seconds for the orientation constraints. We believe we may be able to provide a warm start to the ipopt solver by linearly interpolating joint angles between the current and desired end effector position using IK.
A suggestion given to us is to possibly train a neural network to detect joint collisions. This may give us more joint angle possibilities while keeping computation fast.
We did not end up using terminal constraints in this project, but we would like to use incorporate invariant sets to guarantee recursive feasibility. This was difficult because we were unsure how to compute the maximal invariant set without a standard Q matrix that is the same size as our A matrix.
For possible extensions, we would also like the end effector to avoid obstacles. The simplest case is to avoid spheres and ensure that the end effector remains at least a radius away from its center.
Page updated
Google Sites
Report abuse