diff --git a/app.js b/app.js
index c9a42d9..1ab4223 100755
--- a/app.js
+++ b/app.js
@@ -56,34 +56,37 @@ var app = express();
console.log('use helmet');
app.use(helmet());
-var use_content_security_policy = true
-
+var use_content_security_policy = true;
+// NOTE: operator.html has its own CSP rules that override what is set here
if (use_content_security_policy) {
console.log('using a content security policy');
app.use(helmet.contentSecurityPolicy({
- directives:{
- defaultSrc:["'self'"],
- scriptSrc:["'self'", "'unsafe-inline'",
- 'static.robotwebtools.org',
- 'robotwebtools.org',
- 'webrtc.github.io',
- 'www.gstatic.com',
- 'code.jquery.com',
- 'cdnjs.cloudflare.com',
- 'stackpath.bootstrapcdn.com'],
- connectSrc:["'self'", 'ws://localhost:9090'],
- imgSrc: ["'self'", 'data:'],
- styleSrc:["'self'",
- 'stackpath.bootstrapcdn.com'],
- fontSrc:["'self'"]}}));
+ directives: {
+ defaultSrc:["'self'"],
+ scriptSrc:["'self'", "'unsafe-inline'",
+ 'static.robotwebtools.org',
+ 'robotwebtools.org',
+ 'webrtc.github.io',
+ 'www.gstatic.com',
+ 'code.jquery.com',
+ 'cdnjs.cloudflare.com',
+ 'stackpath.bootstrapcdn.com',
+ 'cdn.jsdelivr.net'],
+ connectSrc:["'self'", 'ws://localhost:9090'],
+ imgSrc: ["'self'", 'data:'],
+ styleSrc:["'self'",
+ 'stackpath.bootstrapcdn.com'],
+ fontSrc:["'self'"]}
+ })
+ );
} else {
// Disable the content security policy. This is helpful during
// development, but risky when deployed.
console.log('WARNING: Not using a content security policy. This risky when deployed!');
app.use(
- helmet({
- contentSecurityPolicy: false,
- })
+ helmet({
+ contentSecurityPolicy: false,
+ })
);
}
diff --git a/operator/operator.css b/operator/operator.css
index 4912a66..e52099c 100644
--- a/operator/operator.css
+++ b/operator/operator.css
@@ -63,6 +63,15 @@ video {
.foreground {
z-index:2;
cursor: pointer;
+ position: relative;
+}
+
+/* Three.js canvas positioning */
+.foreground > canvas {
+ position: absolute;
+ top: 0;
+ left: 0;
+ pointer-events: none;
}
@@ -343,7 +352,7 @@ video {
background: transparent;
border: solid 3px #000000;
border-radius: 3px;
- /*background-image: linear-gradient(to bottom, #4fc9ee, #0000ff); /*#3aa2d0);*/*/
+ /*background-image: linear-gradient(to bottom, #4fc9ee, #0000ff); /*#3aa2d0);*/
/*box-shadow: inset 0 1px rgba(255, 255, 255, 0.5), 0 0 2px rgba(0, 0, 0, 0.2);*/
/*transition: left 0.15s ease-out;*/
transition: left 0.1s ease-out;
diff --git a/operator/operator.html b/operator/operator.html
index c82f010..2aed2c0 100644
--- a/operator/operator.html
+++ b/operator/operator.html
@@ -5,13 +5,17 @@
+
-
+
-
+
@@ -232,7 +236,10 @@ Settings
-
+
+
+
+
diff --git a/operator/operator_ui_regions.js b/operator/operator_ui_regions.js
index a37d5b2..3022df9 100644
--- a/operator/operator_ui_regions.js
+++ b/operator/operator_ui_regions.js
@@ -1,44 +1,37 @@
'use strict';
-// TODO: This might be redundant with 'interfaceMode' remove after checking
-var strokeOpacity = 0.0;
-var w = videoDimensions.h;
-var h = videoDimensions.w;
-
-
-function setCameraViewPreset() {
- if (panTiltCameraVideoControl.currentMode != null)
- setCameraView(panTiltCameraVideoControl.currentMode);
-}
-
-
-var navigationVideoControl = new VideoControl('navigationVideo', 'nav');
-var manipulationVideoControl = new VideoControl('manipulationVideo', 'manip');
-
/*
* Class for a video stream visualization with an overlay
* This is redundant at the moment but will be necessary
* soon, when we want to have more than one video stream
* visualization.
*/
-function VideoControl(videoId, currentMode=null) {
- this.videoId = videoId;
- this.combinedSVG = document.getElementById(videoId + "Overlay");
- this.combinedSVG.setAttribute('viewBox', '0 0 ' + w + ' ' + h);
- this.overlays = {}; // key is mode id, value is the Overlay object
- this.currentMode = currentMode;
- this.videoDiv = document.getElementById(videoId + "Div");
- this.video = document.getElementById(videoId);
- this.video.setAttribute("height", h);
- this.video.setAttribute("width", w);
- this.isActive = false;
-
- this.addOverlay = function(overlay) {
- this.overlays[overlay.modeId] = overlay;
- this.combinedSVG.appendChild(overlay.svg);
- }
-
- this.setMode = function(modeId) {
+class VideoControl {
+ constructor(videoId, currentMode=null) {
+ this.videoId = videoId;
+ this.combinedSVG = document.getElementById(videoId + "Overlay");
+ this.combinedSVG.setAttribute('viewBox', '0 0 ' + w + ' ' + h);
+ this.overlays = {}; // key is mode id, values are the Overlay objects
+ this.currentMode = currentMode;
+ this.videoDiv = document.getElementById(videoId + "Div");
+ this.video = document.getElementById(videoId);
+ this.video.setAttribute("height", h);
+ this.video.setAttribute("width", w);
+ this.isActive = false;
+ }
+
+ addOverlay(overlay, type="svg") {
+ if (this.overlays.hasOwnProperty(overlay.modeId)) {
+ this.overlays[overlay.modeId].push(overlay);
+ } else {
+ this.overlays[overlay.modeId] = [overlay];
+ }
+
+ if (type === "svg")
+ this.combinedSVG.appendChild(overlay.svg);
+ }
+
+ setMode(modeId) {
this.currentMode = modeId;
let buttonName = modeId + '_mode_button';
let button = document.getElementById(buttonName);
@@ -50,57 +43,129 @@ function VideoControl(videoId, currentMode=null) {
const modeNames = this.getModeNames();
for (let i in modeNames) {
if (modeNames[i] !== modeId) {
- this.overlays[modeNames[i]].hide();
+ this.overlays[modeNames[i]].forEach( function(overlay) {
+ overlay.hide();
+ });
}
}
- this.overlays[modeId].show();
+ this.overlays[modeId].forEach( function(overlay) {
+ overlay.show();
+ });
}
- this.getModeNames = function() {
+ getModeNames() {
return Object.keys(this.overlays);
}
- this.setActive = function(isActive) {
+ setActive(isActive) {
this.isActive = isActive;
if (this.isActive){
this.videoDiv.style.backgroundColor = "rgba(0,0,0,0.1)";
- this.overlays[this.currentMode].show();
+ this.overlays[this.currentMode].forEach( function(overlay) {
+ overlay.show();
+ });
}
else{
this.videoDiv.style.backgroundColor = "rgba(0,0,0,0.5)";
- this.overlays[this.currentMode].hide();
+ this.overlays[this.currentMode].forEach( function(overlay) {
+ overlay.hide();
+ });
+ }
+ }
+}
+
+class THREEManager {
+ constructor(camera, width, height) {
+ this.camera = camera;
+ this.width = width;
+ this.height = height;
+
+ this.overlays = {};
+ }
+
+ addOverlay(overlay) {
+ this.overlays[overlay.modeId] = {
+ overlay: overlay,
+ render: true
+ };
+ }
+
+ pauseOverlayRender(modeId) {
+ this.overlays[modeId].render = false;
+ this.overlays[modeId].overlay.render();
+ }
+
+ resumeOverlayRender(modeId) {
+ this.overlays[modeId].render = true;
+ this.overlays[modeId].overlay.render();
+ }
+
+ animate() {
+ // TODO: Figure out how to properly pass self into a callback function
+ requestAnimationFrame(() => {
+ this.animate();
+ });
+
+ for (const overlay in this.overlays) {
+ if (this.overlays[overlay].render) {
+ this.overlays[overlay].overlay.render();
+ }
}
}
}
/*
-* Class for a video overlay
+* Base class for a video overlay
*/
-function Overlay(modeId) {
- this.modeId = modeId;
- this.regions = [];
+class Overlay {
+ constructor(modeId) {
+ this.modeId = modeId;
+ }
- this.svg = document.createElementNS('http://www.w3.org/2000/svg', 'svg');
- this.svg.setAttribute('preserveAspectRatio', 'none');
- this.svg.setAttribute('id', modeId + '_ui_overlay');
+ addItem() {
+ console.warn("addItem() should be overridden by the child class");
+ }
- let bgRect = makeRectangle(0, 0, w, h);
- this.curtain = new Region(modeId + '_curtain', null , 'curtain',
- rectToPoly(bgRect), 'white', this.svg, true, 0.5);
+ hide() {
+ console.warn("hide() should be overridden by the child class");
+ }
+
+ show() {
+ console.warn("show() should be overridden by the child class");
+ }
+}
- this.addRegion = function(region) {
+/*
+* Class for an SVG video overlay
+*/
+class OverlaySVG extends Overlay {
+ constructor(modeId) {
+ super(modeId);
+ this.regions = [];
+
+ this.svg = document.createElementNS('http://www.w3.org/2000/svg', 'svg');
+ this.svg.setAttribute('preserveAspectRatio', 'none');
+ this.svg.setAttribute('id', modeId + '_ui_overlay');
+
+ let bgRect = makeRectangle(0, 0, w, h);
+ this.curtain = new Region(modeId + '_curtain', null , 'curtain',
+ rectToPoly(bgRect), 'white', this.svg, true, 0.5);
+
+ this.addRegion(this.curtain);
+ }
+
+ addRegion(region) {
this.regions.push(region);
}
- this.addRegion(this.curtain);
- this.hide = function() {
+ hide() {
for (let i in this.regions) {
this.regions[i].hide();
}
this.curtain.show();
}
- this.show = function() {
+ show() {
for (let i in this.regions) {
this.regions[i].show();
}
@@ -109,28 +174,96 @@ function Overlay(modeId) {
}
/*
-* Class for a video overlay
+* Class for an THREE.js video overlay
+*/
+class OverlayTHREE extends Overlay {
+ constructor(modeId, threeManager) {
+ super(modeId);
+ this.objs = {};
+
+ this.threeManager = threeManager;
+ this.scene = new THREE.Scene();
+ this.renderer = new THREE.WebGLRenderer({ alpha: true });
+ this.renderer.setSize(this.threeManager.width, this.threeManager.height);
+ $(`#${modeId}_ui_overlay`).parent().parent().prepend(this.renderer.domElement);
+
+ this.composer = new POSTPROCESSING.EffectComposer(this.renderer);
+ this.composer.addPass(new POSTPROCESSING.RenderPass(this.scene, this.threeManager.camera));
+
+ this.threeManager.addOverlay(this);
+ }
+
+ addRenderPass(renderPass) {
+ this.composer.addPass(renderPass);
+ }
+
+ addItem(obj) {
+ this.objs[obj.name] = obj;
+ this.scene.add(obj.mesh);
+ }
+
+ hide() {
+ for (const obj in this.objs) {
+ this.objs[obj].hide();
+ }
+ this.threeManager.pauseOverlayRender(this.modeId);
+ }
+
+ show() {
+ for (const obj in this.objs) {
+ this.objs[obj].show();
+ }
+ this.threeManager.resumeOverlayRender(this.modeId);
+ }
+
+ render() {
+ this.composer.render();
+ }
+}
+
+/*
+* Class for an overlay region
*/
-function Region(regionId, fname, label, poly, color, parentSVG, isContinuous=true, fillOpacity=0.0) {
- this.regionId = regionId;
- this.fname = fname;
- this.label = label;
- this.poly = poly;
- this.isContinuous = isContinuous;
- this.parentSVG = parentSVG;
-
- createRegionSVG(this.parentSVG, this.regionId, this.fname, this.label,
- this.poly, color, this.isContinuous, fillOpacity);
-
- this.hide = function() {
+class Region {
+ constructor(regionId, fname, label, poly, color, parentSVG, isContinuous=true, fillOpacity=0.0) {
+ this.regionId = regionId;
+ this.fname = fname;
+ this.label = label;
+ this.poly = poly;
+ this.isContinuous = isContinuous;
+ this.parentSVG = parentSVG;
+
+ createRegionSVG(this.parentSVG, this.regionId, this.fname, this.label,
+ this.poly, color, this.isContinuous, fillOpacity);
+ }
+
+
+ hide() {
document.getElementById(this.regionId).style.display = 'none';
}
- this.show = function() {
+ show() {
document.getElementById(this.regionId).style.display = 'block';
}
}
+class THREEObject {
+ constructor(name, geo, mat) {
+ this.name = name;
+ this.geo = geo;
+ this.mat = mat;
+ this.mesh = new THREE.Mesh(this.geo, this.mat);
+ }
+
+ hide() {
+ this.mesh.visible = false;
+ }
+
+ show() {
+ this.mesh.visible = true;
+ }
+}
+
function setMode(modeId) {
if (modeId == 'nav') {
if (!navigationVideoControl.isActive) {
@@ -140,6 +273,7 @@ function setMode(modeId) {
if (checkbox.checked)
changeGripperFollow(false);
setCameraView('nav');
+ // TODO: Is there some way to set this button list procedurally?
document.getElementById('lookUpNavButton').disabled = false;
document.getElementById('lookLeftNavButton').disabled = false;
document.getElementById('lookRightNavButton').disabled = false;
@@ -189,6 +323,22 @@ function setMode(modeId) {
}
}
+// TODO: This might be redundant with 'interfaceMode' remove after checking
+var strokeOpacity = 0.0;
+var w = videoDimensions.h;
+var h = videoDimensions.w;
+
+
+function setCameraViewPreset() {
+ if (panTiltCameraVideoControl.currentMode != null)
+ setCameraView(panTiltCameraVideoControl.currentMode);
+}
+
+var navigationVideoControl = new VideoControl('navigationVideo', 'nav');
+var manipulationVideoControl = new VideoControl('manipulationVideo', 'manip');
+
+var threeManager = new THREEManager(new THREE.PerspectiveCamera(69, w/h, 0.1, 1000), w, h);
+
function createUiRegions(debug) {
@@ -203,7 +353,7 @@ function createUiRegions(debug) {
// navigation
/////////////////////////
- let navOverlay = new Overlay('nav');
+ let navOverlay = new OverlaySVG('nav');
// Big rectangle at the borders of the video
let bgRect = makeRectangle(0, 0, w, h);
let smRect = makeSquare((w/2.0)-(w/20.0), (h*(3.0/4.0))-(h/20.0), w/10.0, h/10.0);
@@ -225,6 +375,27 @@ function createUiRegions(debug) {
navOverlay.addRegion(new Region('nav_ccw_region', 'turnCCW' , 'turn 90 degrees CCW',
rectToPoly(rightRect), color, navOverlay.svg, false));
navigationVideoControl.addOverlay(navOverlay);
+
+ let navOverlayTHREE = new OverlayTHREE('nav', threeManager);
+
+ navOverlayTHREE.addItem(new THREEObject(
+ 'reach_visualization_circle',
+ new THREE.CircleGeometry(0.52, 32), // The arm has a 52 centimeter reach (source: https://hello-robot.com/product#:~:text=range%20of%20motion%3A%2052cm)
+ new THREE.MeshBasicMaterial({color: 'rgb(246, 179, 107)', transparent: true, opacity: 0.25}),
+ ));
+ var outlineEffect = new POSTPROCESSING.OutlineEffect(
+ navOverlayTHREE.scene,
+ navOverlayTHREE.threeManager.camera,
+ {visibleEdgeColor: 0xff9900});
+ var outlineEffectPass = new POSTPROCESSING.EffectPass(
+ navOverlayTHREE.threeManager.camera,
+ outlineEffect
+ );
+ outlineEffectPass.renderToScreen = true;
+ outlineEffect.selectObject(navOverlayTHREE.objs.reach_visualization_circle.mesh);
+ navOverlayTHREE.addRenderPass(outlineEffectPass);
+ navigationVideoControl.addOverlay(navOverlayTHREE, "threejs");
+
navigationVideoControl.setMode("nav");
navigationVideoControl.setActive(true);
@@ -233,7 +404,7 @@ function createUiRegions(debug) {
// manipulation
/////////////////////////
- let armOverlay = new Overlay('manip');
+ let armOverlay = new OverlaySVG('manip');
bgRect = makeRectangle(0, h/5.0, w, h-h/5.0);
// Small rectangle at the top of the middle of the video
@@ -423,7 +594,7 @@ function createUiV1Regions(debug) {
/////// UTILITY FUNCTIONS //////////
-function createRegionSVG(parentSVG, id, fname, title, poly, color, isContinuous, fillOpacity, stroke_width = 2, stroke_opacity = 0.3){
+function createRegionSVG(parentSVG, id, fname, title, poly, color, isContinuous, fillOpacity, stroke_width = 2, stroke_opacity = 0.3) {
let path = document.createElementNS('http://www.w3.org/2000/svg','path');
path.setAttribute('fill-opacity', '0.0');
path.setAttribute('stroke-opacity', '1.0');
@@ -501,6 +672,6 @@ function drawText(elementID, text, x, y, font_size=100, center=false, color='whi
document.getElementById(elementID).appendChild(txt);
}
-createUiRegions(true); // debug = true or false
-
+createUiRegions(true); // debug = true or false
+threeManager.animate();
diff --git a/robot/robot.html b/robot/robot.html
index 00c480c..97c6e56 100644
--- a/robot/robot.html
+++ b/robot/robot.html
@@ -43,6 +43,8 @@
+
+
@@ -58,6 +60,7 @@
// When the robot and the operator are first connected, switch to navigation mode.
//console.log('starting in navigation mode')
//turnModeOn('nav')
+ sendTfs();
}
diff --git a/robot/ros_connect.js b/robot/ros_connect.js
index 1ace6cf..212be3b 100644
--- a/robot/ros_connect.js
+++ b/robot/ros_connect.js
@@ -9,8 +9,6 @@ var img = document.createElement("IMG");
img.style.visibility = 'hidden';
var rosJointStateReceived = false;
var jointState = null;
-var rosRobotStateReceived = false;
-var robotState = null;
var isWristFollowingActive = false;
var session_body = {ws:null, ready:false, port_details:{}, port_name:"", version:"", commands:[], hostname:"", serial_ports:[]};
@@ -146,11 +144,23 @@ var tfClient = new ROSLIB.TFClient({
var link_gripper_finger_left_tf;
tfClient.subscribe('link_gripper_finger_left', function(tf) {
link_gripper_finger_left_tf = tf;
+ sendData({
+ type: 'sensor',
+ subtype: 'gripper',
+ name: 'transform',
+ value: tf
+ });
});
-var link_head_tilt_tf;
-tfClient.subscribe('link_head_tilt', function(tf) {
- link_head_tilt_tf = tf;
+var camera_color_frame_tf;
+tfClient.subscribe('camera_color_frame', function(tf) {
+ camera_color_frame_tf = tf;
+ sendData({
+ type: 'sensor',
+ subtype: 'head',
+ name: 'transform',
+ value: tf
+ });
});
var base_tf;
@@ -158,6 +168,26 @@ tfClient.subscribe('odom', function(tf) {
base_tf = tf;
});
+function sendTfs() {
+ if (link_gripper_finger_left_tf) {
+ sendData({
+ type: 'sensor',
+ subtype: 'gripper',
+ name: 'transform',
+ value: link_gripper_finger_left_tf
+ });
+ }
+
+ if (camera_color_frame_tf) {
+ sendData({
+ type: 'sensor',
+ subtype: 'head',
+ name: 'transform',
+ value: camera_color_frame_tf
+ });
+ }
+}
+
var trajectoryClients = {}
trajectoryClients.main = new ROSLIB.ActionClient({
ros : ros,
@@ -416,17 +446,6 @@ function baseTurn(ang_deg, vel) {
//sendCommandBody({type: "base",action:"turn", ang:ang_deg, vel:vel});
}
-function limitAngle(rad) {
- while (rad > Math.PI) {
- rad -= Math.PI;
- }
- while (rad < -Math.PI) {
- rad += Math.PI;
- }
-
- return rad;
-}
-
function getJointEffort(jointStateMessage, jointName) {
// TODO: Make this work in simulation also
var jointIndex = jointStateMessage.name.indexOf(jointName)
diff --git a/shared/send_recv_av.js b/shared/send_recv_av.js
index 0e077d2..bc9c7d2 100644
--- a/shared/send_recv_av.js
+++ b/shared/send_recv_av.js
@@ -379,26 +379,26 @@ function dataChannelCallback(event) {
function onReceiveMessageCallback(event) {
var obj = safelyParseJSON(event.data);
switch(obj.type) {
- case 'command':
- objects_received.push(obj);
- console.log('Received Data: ' + event.data);
- //console.log('Received Object: ' + obj);
- executeCommand(obj);
- break;
- case 'sensor':
- // unless being recorded, don't store or write information to the console due to high
- // frequency and large amount of data (analogous to audio and video).
- if (recordOn && addToSensorLog) {
- addToSensorLog(obj);
- }
- receiveSensorReading(obj);
- break;
- default:
- console.log('*******************************************************');
- console.log('UNRECOGNIZED MESSAGE TYPE RECEIVED, SO DOING NOTHING...');
- console.log('Received Data: ' + event.data);
- console.log('Received Object: ' + obj);
- console.log('*******************************************************');
+ case 'command':
+ objects_received.push(obj);
+ console.log('Received Data: ' + event.data);
+ //console.log('Received Object: ' + obj);
+ executeCommand(obj);
+ break;
+ case 'sensor':
+ // unless being recorded, don't store or write information to the console due to high
+ // frequency and large amount of data (analogous to audio and video).
+ if (recordOn && addToSensorLog) {
+ addToSensorLog(obj);
+ }
+ receiveSensorReading(obj);
+ break;
+ default:
+ console.log('*******************************************************');
+ console.log('UNRECOGNIZED MESSAGE TYPE RECEIVED, SO DOING NOTHING...');
+ console.log('Received Data: ' + event.data);
+ console.log('Received Object: ' + obj);
+ console.log('*******************************************************');
}
}
diff --git a/shared/sensors.js b/shared/sensors.js
index 3ddc4e4..f2c4d0d 100644
--- a/shared/sensors.js
+++ b/shared/sensors.js
@@ -7,12 +7,12 @@ var liftSensors = {
// adjust for the effort needed to hold the arm in place
// against gravity
var adjusted_value = value - 53.88;
- var armUpRegion1 = document.querySelector('#low_arm_up_region');
- var armUpRegion2 = document.querySelector('#high_arm_up_region');
+ var armUpRegion1 = document.getElementById('low_arm_up_region');
+ var armUpRegion2 = document.getElementById('high_arm_up_region');
+
+ var armDownRegion1 = document.getElementById('low_arm_down_region');
+ var armDownRegion2 = document.getElementById('high_arm_down_region');
- var armDownRegion1 = document.querySelector('#low_arm_down_region');
- var armDownRegion2 = document.querySelector('#high_arm_down_region');
-
var redRegion1;
var redRegion2;
@@ -48,11 +48,11 @@ var liftSensors = {
var armSensors = {
"arm_effort": function(value) {
- var armExtendRegion1 = document.querySelector('#low_arm_extend_region');
- var armExtendRegion2 = document.querySelector('#high_arm_extend_region');
+ var armExtendRegion1 = document.getElementById('low_arm_extend_region');
+ var armExtendRegion2 = document.getElementById('high_arm_extend_region');
- var armRetractRegion1 = document.querySelector('#low_arm_retract_region');
- var armRetractRegion2 = document.querySelector('#high_arm_retract_region');
+ var armRetractRegion1 = document.getElementById('low_arm_retract_region');
+ var armRetractRegion2 = document.getElementById('high_arm_retract_region');
var redRegion1;
var redRegion2;
@@ -88,92 +88,185 @@ var armSensors = {
var wristSensors = {
"yaw_torque": function(value) {
- var yawInRegion = document.querySelector('#hand_in_region');
- var yawOutRegion = document.querySelector('#hand_out_region');
- var redRegion;
- var nothingRegion;
- if (value > 0.0) {
- redRegion = yawOutRegion;
- nothingRegion = yawInRegion;
- } else {
- redRegion = yawInRegion;
- nothingRegion = yawOutRegion;
- }
- redRegion.setAttribute('fill', 'red');
- // make the torque positive and multiply it by a factor to
- // make sure the video will always be visible even with
- var redOpacity = Math.abs(value) * 0.015;
- redRegion.setAttribute('fill-opacity', redOpacity);
- nothingRegion.setAttribute('fill-opacity', 0.0);
+ var yawInRegion = document.getElementById('hand_in_region');
+ var yawOutRegion = document.getElementById('hand_out_region');
+ var redRegion;
+ var nothingRegion;
+ if (value > 0.0) {
+ redRegion = yawOutRegion;
+ nothingRegion = yawInRegion;
+ } else {
+ redRegion = yawInRegion;
+ nothingRegion = yawOutRegion;
+ }
+ redRegion.setAttribute('fill', 'red');
+ // make the torque positive and multiply it by a factor to
+ // make sure the video will always be visible even with
+ var redOpacity = Math.abs(value) * 0.015;
+ redRegion.setAttribute('fill-opacity', redOpacity);
+ nothingRegion.setAttribute('fill-opacity', 0.0);
},
"bend_torque": function(value) {
- var bendUpRegion = document.querySelector('#wrist_bend_up_region');
- var bendDownRegion = document.querySelector('#wrist_bend_down_region');
- var redRegion;
- var nothingRegion;
- if (value > 0.0) {
- redRegion = bendUpRegion;
- nothingRegion = bendDownRegion;
- } else {
- redRegion = bendDownRegion;
- nothingRegion = bendUpRegion;
- }
- redRegion.setAttribute('fill', 'red');
- // make the torque positive and multiply it by a factor to
- // make sure the video will always be visible even with
- var redOpacity = Math.abs(value) * 0.8;
- redRegion.setAttribute('fill-opacity', redOpacity);
- nothingRegion.setAttribute('fill-opacity', 0.0);
+ var bendUpRegion = document.getElementById('wrist_bend_up_region');
+ var bendDownRegion = document.getElementById('wrist_bend_down_region');
+ var redRegion;
+ var nothingRegion;
+ if (value > 0.0) {
+ redRegion = bendUpRegion;
+ nothingRegion = bendDownRegion;
+ } else {
+ redRegion = bendDownRegion;
+ nothingRegion = bendUpRegion;
+ }
+ redRegion.setAttribute('fill', 'red');
+ // make the torque positive and multiply it by a factor to
+ // make sure the video will always be visible even with
+ var redOpacity = Math.abs(value) * 0.8;
+ redRegion.setAttribute('fill-opacity', redOpacity);
+ nothingRegion.setAttribute('fill-opacity', 0.0);
},
"roll_torque": function(value) {
- var rollLeftRegion = document.querySelector('#wrist_roll_left_region');
- var rollRightRegion = document.querySelector('#wrist_roll_right_region');
- var redRegion;
- var nothingRegion;
- if (value > 0.0) {
- redRegion = rollLeftRegion;
- nothingRegion = rollRightRegion;
- } else {
- redRegion = rollRightRegion;
- nothingRegion = rollLeftRegion;
- }
- redRegion.setAttribute('fill', 'red');
- // make the torque positive and multiply it by a factor to
- // make sure the video will always be visible even with
- var redOpacity = Math.abs(value) * 0.8;
- redRegion.setAttribute('fill-opacity', redOpacity);
- nothingRegion.setAttribute('fill-opacity', 0.0);
+ var rollLeftRegion = document.getElementById('wrist_roll_left_region');
+ var rollRightRegion = document.getElementById('wrist_roll_right_region');
+ var redRegion;
+ var nothingRegion;
+ if (value > 0.0) {
+ redRegion = rollLeftRegion;
+ nothingRegion = rollRightRegion;
+ } else {
+ redRegion = rollRightRegion;
+ nothingRegion = rollLeftRegion;
+ }
+ redRegion.setAttribute('fill', 'red');
+ // make the torque positive and multiply it by a factor to
+ // make sure the video will always be visible even with
+ var redOpacity = Math.abs(value) * 0.8;
+ redRegion.setAttribute('fill-opacity', redOpacity);
+ nothingRegion.setAttribute('fill-opacity', 0.0);
}
}
var gripperSensors = {
- "gripper_torque": function(value) {
- var handCloseRegion = document.querySelector('#hand_close_region');
- var handOpenRegion = document.querySelector('#hand_open_region');
- var redRegion;
- var nothingRegion;
- if (value > 0.0) {
- redRegion = handOpenRegion;
- nothingRegion = handCloseRegion;
- } else {
- redRegion = handCloseRegion;
- nothingRegion = handOpenRegion;
+ "gripper_torque": function(value) {
+ var handCloseRegion = document.getElementById('hand_close_region');
+ var handOpenRegion = document.getElementById('hand_open_region');
+ var redRegion;
+ var nothingRegion;
+ if (value > 0.0) {
+ redRegion = handOpenRegion;
+ nothingRegion = handCloseRegion;
+ } else {
+ redRegion = handCloseRegion;
+ nothingRegion = handOpenRegion;
+ }
+ redRegion.setAttribute('fill', 'red');
+ // make the torque positive and multiply it by a factor to
+ // make sure the video will always be visible even with
+ var redOpacity = Math.abs(value) * 0.015;
+ redRegion.setAttribute('fill-opacity', redOpacity);
+ nothingRegion.setAttribute('fill-opacity', 0.0);
+ },
+ "transform": function (value) {
+ //navModeObjects.cube.position.copy(rosPostoTHREE(value.translation).add(positionOffset));
}
- redRegion.setAttribute('fill', 'red');
- // make the torque positive and multiply it by a factor to
- // make sure the video will always be visible even with
- var redOpacity = Math.abs(value) * 0.015;
- redRegion.setAttribute('fill-opacity', redOpacity);
- nothingRegion.setAttribute('fill-opacity', 0.0);
- }
}
+// Camera Position Information
+const global_rotation_point = new THREE.Vector3(
+ -0.001328,
+ 0,
+ -0.053331
+);
+
+
+const global_reference_point = new THREE.Vector3(
+ -0.001328,
+ 0.027765,
+ -0.053331
+);
+
+const global_target_point = new THREE.Vector3(
+ 0.037582,
+ -0.002706,
+ 0.019540000000000113
+).add(global_reference_point);
+
+var reference_to_rotation_offset = global_rotation_point.clone().sub(global_reference_point);
+var rotation_to_target_offset = global_target_point.clone().sub(global_rotation_point);
+
+var headSensors = {
+ "transform": function (value) {
+ // Update the rotation and translation of the THREE.js camera to match the physical one
+
+ var q_ros_space = new THREE.Quaternion(value.rotation.x, value.rotation.y, value.rotation.z, value.rotation.w);
+
+ var order = 'XYZ'
+ var e = new THREE.Euler(0, 0, 0, order);
+ e.setFromQuaternion(q_ros_space, order);
+
+ var q_inverse = q_ros_space.clone().invert();
+
+ var reference_point = new THREE.Vector3(value.translation.x, value.translation.y, value.translation.z);
+ // z in global space is y in ros space
+ var rotated_reference_to_rotation_offset = reference_to_rotation_offset.clone().applyEuler(new THREE.Euler(0, -e.z, e.y, 'XZY'));
+
+ // TODO: Shouldn't this always be static, meaning that the previous math is unnecessary?
+ var rotation_point = reference_point.clone().add(rotated_reference_to_rotation_offset);
+
+ var rotated_rotation_offset_to_target_offset = rotation_to_target_offset.clone().applyEuler(new THREE.Euler(0, -e.z, e.y, 'XZY'));
+
+ var target_point = rotation_point.clone().add(rotated_rotation_offset_to_target_offset);
+
+ //threeManager.camera.position.copy(target_point);
+ threeManager.camera.position.copy(rosPostoTHREE(value.translation));
+
+ var e_three_space = rosEulerToTHREE(e, 'YZX');
+ threeManager.camera.rotation.copy(e_three_space);
+
+ /*
+ console.log("e_ros_space", e);
+ console.log("q_ros_space", q_ros_space);
+ console.log("q_inverse", q_inverse);
+ console.log("rotated_reference_to_rotation_offset", rotated_reference_to_rotation_offset);
+ console.log("reference_point", reference_point);
+ console.log("rotation_point", rotation_point.clone().multiplyScalar(100));
+ console.log("target_point", target_point);
+ console.log("e_three_space", e_three_space);
+ */
+ }
+}
+
+function rosPostoTHREE(p) {
+ return new THREE.Vector3(p.x, -p.y, p.z);
+}
+
+function rosEulerToTHREE(e, order) {
+ return new THREE.Euler(
+ e.z+(Math.PI/2),
+ 0,
+ e.y+(Math.PI/2),
+ order
+ )
+}
+
+function limitAngle(rad, lower = -Math.PI/2, upper = Math.PI/2) {
+ while (rad > upper) {
+ rad -= 2*Math.PI;
+ }
+ while (rad < lower) {
+ rad += 2*Math.PI;
+ }
+
+ return rad;
+}
+
var sensors = {
"drive": driveSensors,
"lift": liftSensors,
"arm": armSensors,
"wrist": wristSensors,
- "gripper": gripperSensors
+ "gripper": gripperSensors,
+ "head": headSensors
}
function receiveSensorReading(obj) {