00001
00002 import numpy as np
00003 import math
00004
00005 height = 0.0
00006 upper_arm_length = 0.334
00007 forearm_length = 0.288
00008 hand_length = 0.12
00009
00010 ee_location = np.matrix([0., -upper_arm_length-forearm_length-hand_length, height]).T
00011
00012 bod_color = [[0.4, 0.4, 0.4, 1], [0.8, 0.8, 0.8, 1], [0.33, 0.33, 0.33, 1]]
00013 bod_num_links = 3
00014
00015 bod_mass = [2.3, 1.32, 0.7]
00016
00017 bod_names = ['link1', 'link2', 'link3']
00018
00019 b_jt_axis = [[0.,0.,1.],[0.,0.,1.], [0.,0.,1.]]
00020 b_jt_anchor = [[0., 0., height], [0., -upper_arm_length, height],
00021 [0., -upper_arm_length-forearm_length, height]]
00022 b_jt_kp = [20., 15., 10.]
00023 b_jt_kd = [3., 2., 1.]
00024 b_jt_limits_max = np.radians([162, 159, 90]).tolist()
00025 b_jt_limits_min = np.radians([-63, 0, -45]).tolist()
00026 b_jt_axis = [[0.,0.,1.],[0.,0.,1.], [0.,0.,1.]]
00027 b_jt_attach = [[0, -1], [1, 0], [2,1]]
00028
00029 b_jt_start = np.radians([-30.0, 150, 15]).tolist()
00030
00031
00032 b_jts = {'anchor':b_jt_anchor, 'axis':b_jt_axis, 'jt_lim_max':b_jt_limits_max,
00033 'jt_lim_min':b_jt_limits_min, 'jt_init':b_jt_start, 'jt_attach':b_jt_attach,
00034 'jt_stiffness':b_jt_kp, 'jt_damping':b_jt_kd}
00035
00036
00037
00038 bod_shapes = ['capsule', 'capsule', 'capsule']
00039
00040 dia = 0.03
00041 bod_dimensions = [[dia, dia, upper_arm_length], [dia, dia, forearm_length],
00042 [dia, dia, hand_length-dia/2]]
00043
00044 bod_com_position = [[0., -upper_arm_length/2., height],
00045 [0., -upper_arm_length-forearm_length/2., height],
00046 [0., -upper_arm_length-forearm_length-hand_length/2.+dia/4, height]]
00047
00048 bodies ={'shapes':bod_shapes, 'dim':bod_dimensions, 'num_links':bod_num_links,
00049 'com_pos':bod_com_position, 'mass':bod_mass, 'name':bod_names, 'color':bod_color}
00050