|
41 | 41 | <mesh filename="${mesh}"/> |
42 | 42 | </xacro:macro> |
43 | 43 |
|
44 | | - <xacro:macro name="read_model_data" params="joint_limits_parameters_file kinematics_parameters_file physical_parameters_file visual_parameters_file force_abs_paths"> |
| 44 | + <xacro:macro name="read_model_data" params="joint_limits_parameters_file kinematics_parameters_file physical_parameters_file visual_parameters_file joint_dynamics_parameters_file force_abs_paths"> |
45 | 45 |
|
46 | 46 | <xacro:property name="force_abs_paths" value="${force_abs_paths}" scope="parent"/> |
47 | 47 | <!-- Read .yaml files from disk, load content into properties --> |
48 | 48 | <xacro:property name="config_joint_limit_parameters" value="${xacro.load_yaml(joint_limits_parameters_file)}"/> |
49 | 49 | <xacro:property name="config_kinematics_parameters" value="${xacro.load_yaml(kinematics_parameters_file)}"/> |
50 | 50 | <xacro:property name="config_physical_parameters" value="${xacro.load_yaml(physical_parameters_file)}"/> |
51 | 51 | <xacro:property name="config_visual_parameters" value="${xacro.load_yaml(visual_parameters_file)}"/> |
| 52 | + <xacro:property name="config_joint_dynamics" value="${xacro.load_yaml(joint_dynamics_parameters_file)}"/> |
52 | 53 |
|
53 | 54 | <!-- Extract subsections from yaml dictionaries --> |
54 | 55 | <xacro:property name="sec_limits" value="${config_joint_limit_parameters['joint_limits']}"/> |
|
57 | 58 | <xacro:property name="sec_inertia_parameters" value="${config_physical_parameters['inertia_parameters']}" /> |
58 | 59 | <xacro:property name="sec_mesh_files" value="${config_visual_parameters['mesh_files']}" scope="parent"/> |
59 | 60 | <xacro:property name="sec_kinematics" value="${config_kinematics_parameters['kinematics']}" /> |
| 61 | + <xacro:property name="sec_joint_dynamics" value="${config_joint_dynamics['joint_dynamics']}" /> |
60 | 62 |
|
61 | 63 | <!-- JOINTS LIMIT PARAMETERS --> |
62 | 64 | <xacro:property name="shoulder_pan_lower_limit" value="${sec_limits['shoulder_pan_joint']['min_position']}" scope="parent"/> |
|
84 | 86 | <xacro:property name="wrist_3_velocity_limit" value="${sec_limits['wrist_3_joint']['max_velocity']}" scope="parent"/> |
85 | 87 | <xacro:property name="wrist_3_effort_limit" value="${sec_limits['wrist_3_joint']['max_effort']}" scope="parent"/> |
86 | 88 |
|
| 89 | + <!-- JOINT DYNAMICS PARAMETERS --> |
| 90 | + <xacro:property name="shoulder_pan_damping" value="${sec_joint_dynamics['shoulder_pan_joint']['damping']}" scope="parent"/> |
| 91 | + <xacro:property name="shoulder_pan_friction" value="${sec_joint_dynamics['shoulder_pan_joint']['friction']}" scope="parent"/> |
| 92 | + <xacro:property name="shoulder_lift_damping" value="${sec_joint_dynamics['shoulder_lift_joint']['damping']}" scope="parent"/> |
| 93 | + <xacro:property name="shoulder_lift_friction" value="${sec_joint_dynamics['shoulder_lift_joint']['friction']}" scope="parent"/> |
| 94 | + <xacro:property name="elbow_joint_damping" value="${sec_joint_dynamics['elbow_joint']['damping']}" scope="parent"/> |
| 95 | + <xacro:property name="elbow_joint_friction" value="${sec_joint_dynamics['elbow_joint']['friction']}" scope="parent"/> |
| 96 | + <xacro:property name="wrist_1_damping" value="${sec_joint_dynamics['wrist_1_joint']['damping']}" scope="parent"/> |
| 97 | + <xacro:property name="wrist_1_friction" value="${sec_joint_dynamics['wrist_1_joint']['friction']}" scope="parent"/> |
| 98 | + <xacro:property name="wrist_2_damping" value="${sec_joint_dynamics['wrist_2_joint']['damping']}" scope="parent"/> |
| 99 | + <xacro:property name="wrist_2_friction" value="${sec_joint_dynamics['wrist_2_joint']['friction']}" scope="parent"/> |
| 100 | + <xacro:property name="wrist_3_damping" value="${sec_joint_dynamics['wrist_3_joint']['damping']}" scope="parent"/> |
| 101 | + <xacro:property name="wrist_3_friction" value="${sec_joint_dynamics['wrist_3_joint']['friction']}" scope="parent"/> |
| 102 | + |
87 | 103 | <!-- DH PARAMETERS --> |
88 | 104 | <xacro:property name="d1" value="${sec_dh_parameters['d1']}" scope="parent"/> |
89 | 105 | <xacro:property name="a2" value="${sec_dh_parameters['a2']}" scope="parent"/> |
|
0 commit comments