DrewRobert

"""
Drew Robert
Rho Beta Application
Computer Science, Question 1
"""

# initialize dictionary as a dictionary and final_list as a list
dictionary = {}
final_list = []

DrewRobert

    def map_callback(self, map):
        """
            This is a callback for the /map topic
            :param map: OccupancyGrid
            :return: None
        """
        """
        expanded_map = self.expand(map)
        occo_map = OccupancyGrid()
        occo_map.header = map.header

DrewRobert

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import PoseStamped, Twist, Pose, PoseWithCovariance, Quaternion
import math
import copy
import numpy as np
from nav_msgs.msg import Odometry
import tf
from tf.transformations import euler_from_quaternion
PI = 3.1415926535897

DrewRobert

function [transMat] = fwkin(theta,d,a,alpha)

transMat = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
            sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
            0, sin(alpha), cos(alpha), d;
            0, 0, 0, 1];

end

  

DrewRobert

%% Homework 4
% Drew Robert
% RBE 3001

%% Definitions

%for the purposes of this homework, theta1 and theta2 are labeled as q1(t)
%and q2(t) in all symbolic variable expressions.
syms L1 L2 q1(t) q2(t) pi px py pz Lc1 Lc2 m1 m2 mL g
symVariables = [L1, L2, q1(t), q2(t), px, py, pz, Lc1, Lc2, m1, m2, mL, g];

DrewRobert

%% This script performs live trajectory generation and plotting
% Lab 4 live plotting of task-space velocity vector

clear;
clear java;
%clear import;
clear classes;
vid = hex2dec('3742');
pid = hex2dec('0007');
disp (vid );

DrewRobert

function [pdot] = FwdVel(q,qdot)

jacobian = jacob0(q);

pdot = jacobian*qdot;

end

DrewRobert

%% Homework 3
% Drew Robert
% RBE 3001

%% Definitions
syms A B C q1(t) q2(t) q3(t) pi 
symVariables = [A, B, C, q1(t), q2(t), q3(t)];

%% Question 1d
%input into fwkin in the form: theta, d, a, alpha

DrewRobert

function [transMat] = fwkin(theta,d,a,alpha)

transMat = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
            sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
            0, sin(alpha), cos(alpha), d;
            0, 0, 0, 1];

end

DrewRobert

function angles = ikin(tipPos)
    x = tipPos(1);
    y = tipPos(2);
    z = tipPos(3);
    L1 = 135; %lenth of link 1 (mm)
    L2 = 175; %lenth of link 2 (mm)
    L3 = 169.28; %lenth of link 3 (mm)
    
    theta1 = atan2(y,x); %degree of joint 1 (rad)