00001
00002 import numpy as np
00003 import math
00004 from multi_link_common import *
00005
00006
00007
00008
00009 num_links = 5.0
00010 link_length = total_length/num_links
00011 link_mass = total_mass/num_links
00012
00013 ee_location = np.matrix([0., -total_length, height]).T
00014
00015 bod_shapes = ['capsule', 'capsule', 'capsule', 'capsule', 'capsule']
00016
00017 bod_dimensions = [[0.03, 0.03, link_length]]*5
00018
00019 bod_com_position = [[0., -link_length/2., height],
00020 [0., -3.0/2.0*link_length, height],
00021 [0., -5.0/2.0*link_length, height],
00022 [0., -7.0/2.0*link_length, height],
00023 [0., -9.0/2.0*link_length, height]]
00024
00025 bod_color = [[0.4, 0.4, 0.4, 1], [0.8, 0.8, 0.8, 1], [0.33, 0.33, 0.33, 1], [0.5, 0.5, 0.5, 1], [0.1, 0.1, 0.1, 1]]
00026 bod_num_links = 5
00027 bod_mass = [link_mass]*bod_num_links
00028
00029 bod_names = ['link1', 'link2', 'link3', 'link4', 'link5']
00030 bodies ={'shapes':bod_shapes, 'dim':bod_dimensions, 'num_links':bod_num_links,
00031 'com_pos':bod_com_position, 'mass':bod_mass, 'name':bod_names, 'color':bod_color}
00032
00033 b_jt_axis = [[0.,0.,1.],[0.,0.,1.], [0.,0.,1.], [0.,0.,1.],[0.,0.,1.]]
00034 b_jt_anchor = [[0., 0., height],
00035 [0., -link_length, height],
00036 [0., -2*link_length, height],
00037 [0., -3*link_length, height],
00038 [0., -4*link_length, height]]
00039
00040 b_jt_kp = [30., 19., 10., 5., 2.2]
00041 b_jt_kd = [7., 5., 3., 2., 1.]
00042 b_jt_limits_max = np.radians([180, 120, 120, 120, 120]).tolist()
00043 b_jt_limits_min = np.radians([-180, -120, -120, -120, -120]).tolist()
00044 b_jt_axis = [[0.,0.,1.],[0.,0.,1.], [0.,0.,1.], [0.,0.,1.],[0.,0.,1.]]
00045 b_jt_attach = [[0, -1], [1, 0], [2,1], [3,2], [4,3]]
00046
00047 b_jt_start = [-1.52, 0.447, 1.17, 1.52, 0.637]
00048
00049
00050 b_jts = {'anchor':b_jt_anchor, 'axis':b_jt_axis, 'jt_lim_max':b_jt_limits_max,
00051 'jt_lim_min':b_jt_limits_min, 'jt_init':b_jt_start, 'jt_attach':b_jt_attach,
00052 'jt_stiffness':b_jt_kp, 'jt_damping':b_jt_kd}
00053
00054
00055
00056
00057