diff --git a/css/viewport.css b/css/viewport.css index de02c05..46d0ad7 100644 --- a/css/viewport.css +++ b/css/viewport.css @@ -83,7 +83,7 @@ right: 10px; bottom: 10px; z-index: 20; - display: flex; + display: none; /* flex; */ justify-content: center; align-items: center; width: 1.5em; diff --git a/index.html b/index.html index fa12d27..2e023e8 100644 --- a/index.html +++ b/index.html @@ -25,7 +25,7 @@ - + diff --git a/src/editor/blockly.js b/src/editor/blockly.js index b1d78b9..6e9f5da 100644 --- a/src/editor/blockly.js +++ b/src/editor/blockly.js @@ -13,6 +13,7 @@ Blockly.setLocale(BlocklyDECustom); import './blocks/move' import './blocks/joint_space_pose' import './blocks/task_space_pose' +import './blocks/task_space_position' import './blocks/default_pose' import './blocks/gripper_open' import './blocks/gripper_close' diff --git a/src/editor/blocks/task_space_position.js b/src/editor/blocks/task_space_position.js new file mode 100644 index 0000000..0cefc6a --- /dev/null +++ b/src/editor/blocks/task_space_position.js @@ -0,0 +1,62 @@ +import * as Blockly from "blockly"; + +import ClickableTargetMutator from '../mutators/clickable_target_mutator' +import Simulation from '../../simulator/simulation' + +const fieldKeys = ['X', 'Y', 'Z']; + +Blockly.Blocks["task_space_position"] = { + init: function () { + let i = 0; + this.jsonInit({ + type: "task_space_position", + message0: "x %1 y %2 z %3", + args0: [ + { + "type": "field_number", + "name": fieldKeys[i++], + "value": 0, + "precision": 0.1 + }, + { + "type": "field_number", + "name": fieldKeys[i++], + "value": 0, + "precision": 0.1 + }, + { + "type": "field_number", + "name": fieldKeys[i++], + "value": 0, + "precision": 0.1 + }, + ], + inputsInline: true, + output: "Array", + colour: "%{BKY_MOVEMENT_HEX}", + tooltip: + "Eine Position im Arbeitsraum (definiert über die Position des Endeffektors)", + helpUrl: "", + }); + this.setMutator(new ClickableTargetMutator()); + }, + onClick: function (e) { + Simulation.getInstance(sim => { + const pose = sim.getTaskSpacePose(); + for (let j = 0; j < 3; j++) { + this.setFieldValue(pose[j].toFixed(1), fieldKeys[j]); + } + }); + }, +}; + + +Blockly.JavaScript["task_space_position"] = function (block) { + let ret = '["task_space", '; + for (const key of fieldKeys) { + ret += block.getFieldValue(key) + ', '; + } + ret = ret.slice(0, -1) + ']' + + return [ret, Blockly.JavaScript.ORDER_ATOMIC]; +}; diff --git a/src/simulator/simulation.js b/src/simulator/simulation.js index ee9e9f7..24efc97 100644 --- a/src/simulator/simulation.js +++ b/src/simulator/simulation.js @@ -218,13 +218,21 @@ class TheSimulation { const ikTarget = new Object3D(); ikTarget.position.set(pose[0], pose[1], pose[2]); - ikTarget.setRotationFromEuler(new Euler(pose[3], pose[4], pose[5])); + if (pose.length > 3) { + ikTarget.setRotationFromEuler(new Euler(pose[3], pose[4], pose[5])); + } else { + ikTarget.setRotationFromQuaternion(robot.tcp.object.quaternion); + } const solution = this.ik.solve( ikTarget, robot.tcp.object, robot.ikjoints, - { iterations: 1, jointLimits: robot.interactionJointLimits, apply: false } + { + iterations: 1, + jointLimits: robot.interactionJointLimits, + apply: false + } ); for (let i = 0; i < pose.length; i++) {