Disabled task_space_pose as it's not working properly (robot moves wh…

…erever)

Not happy with the looks of robot info yet, button disabled
Added much simpler task_space_position block (no rotations, also not working yet)

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;

index.html

@@ -25,7 +25,7 @@
 					<category name="R" expanded="true">
 						<block type="move"></block>
 						<block type="joint_space_pose"></block>
-						<block type="task_space_pose"></block>
+						<!--<block type="task_space_pose"></block>  Not working yet! -->
 						<block type="default_pose"></block>
 						<block type="gripper_open"></block>
 						<block type="gripper_close"></block>

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'

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];
+};

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++) {