abhi74k

Robot Perception Fall 25-

Homework 4
Instructions
In this homework assignment, we will continue to develop the remaining building
blocks for a vision-based pick-and-place system. By the end of this assignment, we
will complete the first version of our pick-and-place system – time to get excited!

This homework has three problems. Problem 1, basic concept review. Problem 2,
coding assignment. Problem 3, discussion and analysis. For Problems 1 and 3, please

abhi74k

class StreamStats {
    private final long startTime;
    private long lastReportTime;
    private long lastReportCount = 0;
    private long totalCount = 0;
    private final long reportIntervalMs = 5000; // 5 seconds

    public StreamStats() {
        this.startTime = System.currentTimeMillis();
        this.lastReportTime = startTime;

abhi74k

import torch
import torch_geometric
from torch_geometric.data import Data, DataLoader
from torch_geometric.nn import GCNConv
import torch_geometric.transforms as T
from torch_geometric.utils import to_dense_adj, dense_to_sparse

data = None # Checkout amazongraph.ipynb. I have constructed the data object.

# Cluster the graph

abhi74k

import numpy as np
import matplotlib.pyplot as plt


class ConvexSolver:

    def __init__(self, A, b, c, alpha, beta, eval_grad, eval_hessian, num_iterations=50):
        self.m = A.shape[0]
        self.n = A.shape[1]

abhi74k

include("readclassjson.jl")
data = readclassjson("gauss_fit_data.json")
N = data["N"]
t = data["t"]
y = data["y"]

function f(t, params)
    alpha = params[1]
    mu = params[2]
    sigma = params[3]

abhi74k

from Robot import Robot
import numpy as np


class NonHolonomicMobileRobot(Robot):

    def __init__(self, Q):
        self.Q = Q

    def next_state_without_noise(self, x, u, dt):

abhi74k

import numpy as np

import robot_utils
from NonHolonomicMobileRobot import NonHolonomicMobileRobot
from robot_utils import simulate_trajectory

# Seed it
np.random.seed(123451)

# Sim params

abhi74k

def iterative_ekf_smoother_forward_pass(time_steps, measurements, robot_model, measurement_model, u,
                                        mu_i_trajectory, initial_cov, Q, R):
    ncols = mu_i_trajectory.shape[1]

    predicted_mu_trajectory = [mu_i_trajectory[:, 0]]
    predicted_cov_trajectory = [initial_cov]  # check this

    updated_mu_trajectory = [mu_i_trajectory[:, 0]]  # check this
    updated_cov_trajectory = [initial_cov]

abhi74k

import numpy as np
import scipy.linalg as la
import matplotlib.pyplot as plt
import math 

Jx = 1.
Jy = 5.
Jz = 5.
c = 10
dt = 0.001

abhi74k

import numpy as np

def calculate_log_rigid_body(T):
    R = T[0:3, 0:3]
    p = T[0:3, 3]
    
    if (R == np.eye(3, 3)).all():
        w_matrix = np.zeros((3, 3))
        v = p
    else: