mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2026-02-08 16:05:14 +00:00
Device visualization (#67)
* Update README and MQTTClient for installation instructions and code improvements * feat: 支持local_config启动 add: 增加对crt path的说明,为传入config.py的相对路径 move: web component * add: registry description * add 3d visualization * 完成在main中启动设备可视化 完成在main中启动设备可视化,并输出物料ID:mesh的对应关系resource_model 添加物料模型管理类,遍历物料与resource_model,完成TF数据收集 * 完成TF发布 * 修改模型方向,在yaml中添加变换属性 * 添加物料tf变化时,发送topic到前端 另外修改了物料初始化的方法,防止在tf还未发布时提前建立物料模型与发布话题 * 添加关节发布节点与物料可视化节点进入unilab * 使用json启动plr与3D模型仿真 * feat: node_info_update srv fix: OTDeck cant create * close #12 feat: slave node registry * feat: show machine name fix: host node registry not uploaded * feat: add hplc registry * feat: add hplc registry * fix: hplc status typo * fix: devices/ * 完成启动OT并联动rviz * add 3d visualization * 完成在main中启动设备可视化 完成在main中启动设备可视化,并输出物料ID:mesh的对应关系resource_model 添加物料模型管理类,遍历物料与resource_model,完成TF数据收集 * 完成TF发布 * 修改模型方向,在yaml中添加变换属性 * 添加物料tf变化时,发送topic到前端 另外修改了物料初始化的方法,防止在tf还未发布时提前建立物料模型与发布话题 * 添加关节发布节点与物料可视化节点进入unilab * 使用json启动plr与3D模型仿真 * 完成启动OT并联动rviz * fix: device.class possible null * fix: HPLC additions with online service * fix: slave mode spin not working * fix: slave mode spin not working * 修复rviz位置问题, 修复rviz位置问题, 在无tf变动时减缓发送频率 在backend中添加物料跟随方法 * feat: 多ProtocolNode 允许子设备ID相同 feat: 上报发现的ActionClient feat: Host重启动,通过discover机制要求slaveNode重新注册,实现信息及时上报 * feat: 支持env设置config * fix: running logic * fix: running logic * fix: missing ot * 在main中直接初始化republisher和物料的mesh节点 * 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中 * Device visualization (#14) * add 3d visualization * 完成在main中启动设备可视化 完成在main中启动设备可视化,并输出物料ID:mesh的对应关系resource_model 添加物料模型管理类,遍历物料与resource_model,完成TF数据收集 * 完成TF发布 * 修改模型方向,在yaml中添加变换属性 * 添加物料tf变化时,发送topic到前端 另外修改了物料初始化的方法,防止在tf还未发布时提前建立物料模型与发布话题 * 添加关节发布节点与物料可视化节点进入unilab * 使用json启动plr与3D模型仿真 * 完成启动OT并联动rviz * add 3d visualization * 完成在main中启动设备可视化 完成在main中启动设备可视化,并输出物料ID:mesh的对应关系resource_model 添加物料模型管理类,遍历物料与resource_model,完成TF数据收集 * 完成TF发布 * 修改模型方向,在yaml中添加变换属性 * 添加物料tf变化时,发送topic到前端 另外修改了物料初始化的方法,防止在tf还未发布时提前建立物料模型与发布话题 * 添加关节发布节点与物料可视化节点进入unilab * 使用json启动plr与3D模型仿真 * 完成启动OT并联动rviz * 修复rviz位置问题, 修复rviz位置问题, 在无tf变动时减缓发送频率 在backend中添加物料跟随方法 * fix: running logic * fix: running logic * fix: missing ot * 在main中直接初始化republisher和物料的mesh节点 * 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中 --------- Co-authored-by: zhangshixiang <@zhangshixiang> Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com> * fix: missing hostname in devices_names fix: upload_file for model file * fix: missing paho-mqtt package bump version to 0.9.0 * fix startup add ResourceCreateFromOuter.action * fix type hint * update actions * update actions * host node add_resource_from_outer fix cmake list * pass device config to device class * add: bind_parent_ids to resource create action fix: message convert string * fix: host node should not be re_discovered * feat: resource tracker support dict * feat: add more necessary params * feat: fix boolean null in registry action data * feat: add outer resource * 编写mesh添加action * feat: append resource * add action * feat: vis 2d for plr * fix * fix: browser on rviz * fix: cloud bridge error fallback to local * fix: salve auto run rviz * 初始化两个plate * Device visualization (#22) * add 3d visualization * 完成在main中启动设备可视化 完成在main中启动设备可视化,并输出物料ID:mesh的对应关系resource_model 添加物料模型管理类,遍历物料与resource_model,完成TF数据收集 * 完成TF发布 * 修改模型方向,在yaml中添加变换属性 * 添加物料tf变化时,发送topic到前端 另外修改了物料初始化的方法,防止在tf还未发布时提前建立物料模型与发布话题 * 添加关节发布节点与物料可视化节点进入unilab * 使用json启动plr与3D模型仿真 * 完成启动OT并联动rviz * add 3d visualization * 完成在main中启动设备可视化 完成在main中启动设备可视化,并输出物料ID:mesh的对应关系resource_model 添加物料模型管理类,遍历物料与resource_model,完成TF数据收集 * 完成TF发布 * 修改模型方向,在yaml中添加变换属性 * 添加物料tf变化时,发送topic到前端 另外修改了物料初始化的方法,防止在tf还未发布时提前建立物料模型与发布话题 * 添加关节发布节点与物料可视化节点进入unilab * 使用json启动plr与3D模型仿真 * 完成启动OT并联动rviz * 修复rviz位置问题, 修复rviz位置问题, 在无tf变动时减缓发送频率 在backend中添加物料跟随方法 * fix: running logic * fix: running logic * fix: missing ot * 在main中直接初始化republisher和物料的mesh节点 * 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中 * 编写mesh添加action * add action * fix * fix: browser on rviz * fix: cloud bridge error fallback to local * fix: salve auto run rviz * 初始化两个plate --------- Co-authored-by: zhangshixiang <@zhangshixiang> Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com> * fix: multi channel * fix: aspirate * fix: aspirate * fix: aspirate * fix: aspirate * 提交 * fix: jobadd * fix: jobadd * fix: msg converter * tijiao * add resource creat easy action * identify debug msg * mq client id * 提取lh的joint发布 * unify liquid_handler definition * 修改物料跟随与物料添加逻辑 修改物料跟随与物料添加逻辑 将joint_publisher类移出lh的backends,但仍需要对lh的backends进行一些改写 * Revert "修改物料跟随与物料添加逻辑" This reverts commit498c997ad7. * Reapply "修改物料跟随与物料添加逻辑" This reverts commit3a60d2ae81. * Revert "Merge remote-tracking branch 'upstream/dev' into device_visualization" This reverts commitfa727220af, reversing changes made to498c997ad7. * 修改物料放下时的方法,如果选择 修改物料放下时的方法, 如果选择drop_trash,则删除物料显示 如果选择drop,则让其解除连接 * unilab添加moveit启动 1,整合所有moveit节点到一个move_group中,并整合所有的controller依次激活 2,添加pymoveit2的节点,使用json可直接启动 3,修改机械臂规划方式,添加约束,让冗余关节不会进行过多移动 * 修改物体attach时,多次赋值当前时间导致卡顿问题, * Revert "修改物体attach时,多次赋值当前时间导致卡顿问题," This reverts commit56d45b94f5. * Reapply "修改物体attach时,多次赋值当前时间导致卡顿问题," This reverts commit07d9db20c3. * 添加缺少物料:"plate_well_G12", * add * fix tip resource data * liquid states * change to debug level * Revert "change to debug level" This reverts commit5d9953c3e5. * Reapply "change to debug level" This reverts commit2487bb6ffc. * fix tip resource data * add full device * add moveit yaml * 修复moveit 增加post_init阶段,给予ros_node反向 * remove necessary node * fix moveit action client * remove necessary imports * Update moveit_interface.py * fix handler_key uppercase * json add liquids * fix setup * add * change to "sources" and "targets" for lh * bump version * remove parent's parent link * change arm's name * change name * fix ik error * 修改moveit_interface,并在mqtt上报时发送一个时间戳 * 添加机械臂和移液站 * 添加 * 添加硬件 * update * 添加 --------- Co-authored-by: Harvey Que <Q-Query@outlook.com> Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com> Co-authored-by: zhangshixiang <@zhangshixiang> Co-authored-by: Junhan Chang <changjh@pku.edu.cn>
This commit is contained in:
@@ -0,0 +1,43 @@
|
||||
kinematics:
|
||||
shoulder:
|
||||
x: 0
|
||||
y: 0
|
||||
z: 0.2350
|
||||
roll: 0
|
||||
pitch: 0
|
||||
yaw: 0
|
||||
upperarm:
|
||||
x: 0
|
||||
y: 0
|
||||
z: 0
|
||||
roll: 1.570796326589793
|
||||
pitch: 0
|
||||
yaw: 0
|
||||
forearm:
|
||||
x: -0.9000
|
||||
y: 0
|
||||
z: 0
|
||||
roll: 0
|
||||
pitch: 0
|
||||
yaw: 0
|
||||
wrist_1:
|
||||
x: -0.7720
|
||||
y: 0
|
||||
z: 0.1725
|
||||
roll: 0
|
||||
pitch: 0
|
||||
yaw: 0
|
||||
wrist_2:
|
||||
x: 0
|
||||
y: -0.1280
|
||||
z: 0
|
||||
roll: 1.570796326589793
|
||||
pitch: 0
|
||||
yaw: 0
|
||||
wrist_3:
|
||||
x: 0
|
||||
y: 0.1250
|
||||
z: 0
|
||||
roll: -1.570796326589793
|
||||
pitch: 0
|
||||
yaw: 0
|
||||
@@ -0,0 +1,61 @@
|
||||
joint_limits:
|
||||
shoulder_pan_joint:
|
||||
# acceleration limits are not publicly available
|
||||
has_acceleration_limits: false
|
||||
has_effort_limits: true
|
||||
has_position_limits: true
|
||||
has_velocity_limits: true
|
||||
max_effort: 730.0
|
||||
max_position: !degrees 360.0
|
||||
max_velocity: !degrees 120.0
|
||||
min_position: !degrees -360.0
|
||||
shoulder_lift_joint:
|
||||
# acceleration limits are not publicly available
|
||||
has_acceleration_limits: false
|
||||
has_effort_limits: true
|
||||
has_position_limits: true
|
||||
has_velocity_limits: true
|
||||
max_effort: 730.0
|
||||
max_position: !degrees 360.0
|
||||
max_velocity: !degrees 120.0
|
||||
min_position: !degrees -360.0
|
||||
elbow_joint:
|
||||
# acceleration limits are not publicly available
|
||||
has_acceleration_limits: false
|
||||
has_effort_limits: true
|
||||
has_position_limits: true
|
||||
has_velocity_limits: true
|
||||
max_effort: 430.0
|
||||
max_position: !degrees 180.0
|
||||
max_velocity: !degrees 150.0
|
||||
min_position: !degrees -180.0
|
||||
wrist_1_joint:
|
||||
# acceleration limits are not publicly available
|
||||
has_acceleration_limits: false
|
||||
has_effort_limits: true
|
||||
has_position_limits: true
|
||||
has_velocity_limits: true
|
||||
max_effort: 100.0
|
||||
max_position: !degrees 360.0
|
||||
max_velocity: !degrees 210.0
|
||||
min_position: !degrees -360.0
|
||||
wrist_2_joint:
|
||||
# acceleration limits are not publicly available
|
||||
has_acceleration_limits: false
|
||||
has_effort_limits: true
|
||||
has_position_limits: true
|
||||
has_velocity_limits: true
|
||||
max_effort: 100.0
|
||||
max_position: !degrees 360.0
|
||||
max_velocity: !degrees 210.0
|
||||
min_position: !degrees -360.0
|
||||
wrist_3_joint:
|
||||
# acceleration limits are not publicly available
|
||||
has_acceleration_limits: false
|
||||
has_effort_limits: true
|
||||
has_position_limits: true
|
||||
has_velocity_limits: true
|
||||
max_effort: 100.0
|
||||
max_position: !degrees 360.0
|
||||
max_velocity: !degrees 210.0
|
||||
min_position: !degrees -360.0
|
||||
@@ -0,0 +1,99 @@
|
||||
# Physical parameters
|
||||
|
||||
dh_parameters:
|
||||
d1: 0.2350
|
||||
a2: -0.9000
|
||||
a3: -0.7720
|
||||
d4: 0.1725
|
||||
d5: 0.1280
|
||||
d6: 0.1250
|
||||
|
||||
inertia_parameters:
|
||||
base_mass: 1.5056 # base mass, base inertia, base cog might be incorrect
|
||||
shoulder_mass: 17.04
|
||||
upperarm_mass: 26.927
|
||||
forearm_mass: 8.386
|
||||
wrist_1_mass: 3.095
|
||||
wrist_2_mass: 3.095
|
||||
wrist_3_mass: 0.879
|
||||
|
||||
inertia:
|
||||
base:
|
||||
ixx: 0.0067829
|
||||
ixy: 2.6762E-07
|
||||
ixz: -2.569E-06
|
||||
iyy: 0.0068523
|
||||
iyz: 9.4263E-05
|
||||
izz: 0.010044
|
||||
shoulder:
|
||||
ixx: 0.130294671
|
||||
ixy: -8.271E-05
|
||||
ixz: 0.000106701
|
||||
iyy: 0.085215288
|
||||
iyz: -0.000663345
|
||||
izz: 0.113859158
|
||||
upperarm:
|
||||
ixx: 1.204318595
|
||||
ixy: -0.000466936
|
||||
ixz: -1.78878432
|
||||
iyy: 8.073725654
|
||||
iyz: -0.000305158
|
||||
izz: 6.980457042
|
||||
forearm:
|
||||
ixx: 0.070042956
|
||||
ixy: 9.0183E-05
|
||||
ixz: -0.165661846
|
||||
iyy: 2.160436707
|
||||
iyz: 1.3854E-05
|
||||
izz: 2.109199584
|
||||
wrist_1:
|
||||
ixx: 0.007958413
|
||||
ixy: -3.024E-06
|
||||
ixz: -5.734E-06
|
||||
iyy: 0.006686348
|
||||
iyz: -1.766E-05
|
||||
izz: 0.004840671
|
||||
wrist_2:
|
||||
ixx: 0.007958413
|
||||
ixy: -3.024E-06
|
||||
ixz: 5.734E-06
|
||||
iyy: 0.006686348
|
||||
iyz: 1.766E-05
|
||||
izz: 0.004840671
|
||||
wrist_3:
|
||||
ixx: 0.004065851
|
||||
ixy: 1.5185E-05
|
||||
ixz: -1.1453E-05
|
||||
iyy: 0.004060372
|
||||
iyz: 4.2152E-05
|
||||
izz: 0.001170392
|
||||
|
||||
center_of_mass:
|
||||
base_cog:
|
||||
x: -2.4009E-05
|
||||
y: 0.0011775
|
||||
z: 0.076293
|
||||
shoulder_cog:
|
||||
x: 9.9E-05
|
||||
y: -0.026311
|
||||
z: -0.026723
|
||||
upperarm_cog:
|
||||
x: -0.323686
|
||||
y: -3.9E-05
|
||||
z: 0.200968
|
||||
forearm_cog:
|
||||
x: -0.376841
|
||||
y: 1.5E-05
|
||||
z: 0.070311
|
||||
wrist_1_cog:
|
||||
x: -3.7E-05
|
||||
y: -0.01051
|
||||
z: -0.014865
|
||||
wrist_2_cog:
|
||||
x: 3.7E-05
|
||||
y: 0.01051
|
||||
z: -0.014865
|
||||
wrist_3_cog:
|
||||
x: 0.000242
|
||||
y: -0.001192
|
||||
z: -0.050422
|
||||
@@ -0,0 +1,92 @@
|
||||
mesh_files:
|
||||
base:
|
||||
visual:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/visual/base.dae
|
||||
material:
|
||||
name: "LightGrey"
|
||||
color: "0.7 0.7 0.7 1.0"
|
||||
collision:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/collision/base.stl
|
||||
|
||||
shoulder:
|
||||
visual:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/visual/shoulder.dae
|
||||
material:
|
||||
name: "LightGrey"
|
||||
color: "0.7 0.7 0.7 1.0"
|
||||
collision:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/collision/shoulder.stl
|
||||
|
||||
upperarm:
|
||||
visual:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/visual/upperarm.dae
|
||||
material:
|
||||
name: "LightGrey"
|
||||
color: "0.7 0.7 0.7 1.0"
|
||||
collision:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/collision/upperarm.stl
|
||||
mesh_files:
|
||||
|
||||
forearm:
|
||||
visual:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/visual/forearm.dae
|
||||
material:
|
||||
name: "LightGrey"
|
||||
color: "0.7 0.7 0.7 1.0"
|
||||
collision:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/collision/forearm.stl
|
||||
|
||||
wrist_1:
|
||||
visual:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/visual/wrist1.dae
|
||||
material:
|
||||
name: "LightGrey"
|
||||
color: "0.7 0.7 0.7 1.0"
|
||||
collision:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/collision/wrist1.stl
|
||||
|
||||
wrist_2:
|
||||
visual:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/visual/wrist2.dae
|
||||
material:
|
||||
name: "LightGrey"
|
||||
color: "0.7 0.7 0.7 1.0"
|
||||
collision:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/collision/wrist2.stl
|
||||
|
||||
wrist_3:
|
||||
visual:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/visual/wrist3.dae
|
||||
material:
|
||||
name: "LightGrey"
|
||||
color: "0.7 0.7 0.7 1.0"
|
||||
collision:
|
||||
mesh:
|
||||
package: eli_cs_robot_description
|
||||
path: meshes/cs620/collision/wrist3.stl
|
||||
Reference in New Issue
Block a user