#!/usr/bin/env python

import rospy

import numpy

from numpy import matrix

from baxter_interface import CameraController

from sensor_msgs.msg import Image

import struct

import sys

import tf

import math

import time

import convert_notes

import numpy

from numpy import *

from numpy.linalg import *

from scipy import linalg

from tf import transformations

from sensor_msgs.msg import (

JointState

)

from baxter_core_msgs.msg import(

JointCommand,

)

import util

import time

def C(theta):

return math.cos(theta)

def S(theta):

return math.sin(theta)

def R_norm(R):

return numpy.column_stack((numpy.transpose(aa),numpy.transpose(bb), numpy.transpose(cc)))

def R2rpy(R):

return [[alpha],[beta],[gamma]]

def R2axisang(R): # note only returns k in this case

return k

def mag_speed(speed_max, speed_min, delta, eps, Lambda):

return (speed_max - speed_min) / (eps*(Lambda-1)) * (delta-eps) + speed_min

def A_calc(theta,a,alpha,d):

return [[C(theta),-1*S(theta)*C(alpha),S(theta)*S(alpha),a*C(theta)],[S(theta),C(theta)*C(alpha),-1*C(theta)*S(alpha),a*S(theta)],[0,S(alpha),C(alpha),d],[0,0,0,1]]

def DH(THETA, A, ALPHA, D):

return T

def hat(H):

return [[0,-H[2],H[1]],[H[2],0,-H[0]],[-H[1],H[0],0]]

def xi_rev(w,q):

return numpy.vstack((numpy.transpose(matrix(numpy.cross(-w, q))), numpy.transpose(matrix(w))))

def baxter_fk(q):

return H

def generate_joint_names_for_arm(arm):

exts = ['s0','s1','e0','e1','w0','w1','w2']

return map(lambda ext: arm + '_' + ext, exts)

def send_to_joint_vals(q, arm='right'):

return (qc[2], qc[3], qc[0], qc[1], qc[4], qc[5], qc[6])

def getpose():

return (angles, T08)

def Jv(rho,z,on,o):

#print "inside Jv =",numpy.add(numpy.transpose([numpy.multiply(rho,numpy.cross(z,numpy.subtract(on,o)))]),numpy.transpose([numpy.multiply(rho-1,z)]))

return numpy.add(numpy.transpose([numpy.multiply(rho,numpy.cross(z,numpy.subtract(on,o)))]),numpy.transpose([numpy.multiply(rho-1,z)])) # Jv = z_(i-1)X(O_n - O_(i-1)) for revolute joint, i, and Jv = z_(i-1) for prismatic joint, i.

def Jw(rho,z):

return numpy.transpose([numpy.multiply(rho,z)]) # Jw = z for revolute joint, i, and Jw = [0;0;0] for prismatic joint, i.

def Jg(Jv,Jh):

return numpy.vstack((Jv,Jh)) # Put the two halves of the Gemetric Jacobian togeter, could be called Jh = Jg(Jv(rho,z,on,o),Jw(rho,z));

def calc_Jg(frames):

return J_g

def inv_kin(Rot,XYZ,q):

return q

angular_tol = .1

qMax = [.89, 1.047, 3.028, 2.618, 3.059, 2.094, 3.059]

qMin = [-2.461, -2.147, -3.028, -.052, -3.059, -1.571, -3.059]

def get_joint_values(arm):

return qc

def get_trans(arm):

return t

def rot_pos(trans):

return (trans[0:3,0:3], trans[0:3,3])

def mid_to_up_or_down(down, trans):

}

# init_pos is the position of baxter at the first key, pressing down, as joints

# key_delta is a transformation matrix, as is up_z

def generate_q_for_keys(cur_pos, cur_trans, num_keys, key_delta, up_z, down_z, arm):

return (mids, downs, ups)

key_delta_pos = [0, .0762, 0]

# key_delta_pos = [0, .03, 0]

key_delta = util.transformation(numpy.eye(3), key_delta_pos)

up_z_delta_pos = [0, 0, -.05]

up_z_delta = util.transformation(numpy.eye(3), up_z_delta_pos)

down_z_delta_pos = [0, 0, .015]

down_z_delta = util.transformation(numpy.eye(3), down_z_delta_pos)

NUM_KEYS = 8

def move_to_key(from_key, to_key, key_up_qs, key_down_qs):

send_to_joint_vals(key_down_qs[to_key])

def step_off_key(from_key, key_up_qs):

send_to_joint_vals(key_up_qs[from_key])

def run_song(songfile, cur_key, key_up_qs, key_down_qs):

control_rate.sleep()

initial_key = 0

def main():

print('bye!')

if __name__ == '__main__':

main()