Skip to content

Commit

Permalink
clean up three.js transformations and reduce network traffic
Browse files Browse the repository at this point in the history
  • Loading branch information
@kavidey
kavidey committed Jul 4, 2021
1 parent e30ed57 commit 363c8ca
Show file tree
Hide file tree
Showing 3 changed files with 20 additions and 48 deletions.
5 changes: 4 additions & 1 deletion operator/operator_ui_regions.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -317,7 +317,7 @@ function arrangeOverlays(key) {

///////////////////////////////////////////////////////

const positionOffset = new THREE.Vector3(0.05,0,0); //(0, -0.047, -0.02);
const positionOffset = new THREE.Vector3(0, 0, 0); //(0, -0.047, -0.02);

function THREEinit() {
// General THREE.js setup
Expand Down Expand Up @@ -345,6 +345,7 @@ function THREEinit() {
// navMode
navModeObjects = {};

// Reach overlay circle
var geo = 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)
var mat = new THREE.MeshBasicMaterial({color: 'rgb(246, 179, 107)', transparent: true, opacity: 0.25});
var circle = new THREE.Mesh(geo, mat);
Expand All @@ -353,9 +354,11 @@ function THREEinit() {
navModeObjects.circle = circle;
outlineEffect.selectObject(circle);

// Debugging cube
geo = new THREE.BoxGeometry(0.05, 0.05, 0.05);
mat = new THREE.MeshBasicMaterial( { color: 0x00ff00 } );
var cube = new THREE.Mesh(geo, mat);
cube.visible = false;
THREEscene.add(cube);
navModeObjects.cube = cube;

Expand Down
40 changes: 9 additions & 31 deletions robot/ros_connect.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,16 +77,6 @@ jointStateTopic.subscribe(function(message) {
// float64[] velocity
// float64[] effort
//imageTopic.unsubscribe()

sendData({
type: 'sensor',
subtype: 'head',
name: 'joint_transform',
value: {
pan: getJointValue(jointState, 'joint_head_pan'),
tilt: getJointValue(jointState, 'joint_head_tilt')
}
});
});

var tfClient = new ROSLIB.TFClient({
Expand All @@ -107,9 +97,9 @@ tfClient.subscribe('link_gripper_finger_left', function(tf) {
});
});

var link_head_tilt_tf;

This comment has been minimized.

Sign in to view

Copy link
@mayacakmak

mayacakmak Jul 9, 2021

@kaviMD Heads up, this breaks the gripper following functionality! I'm adding it back in the master branch
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',
Expand All @@ -133,24 +123,12 @@ function sendTfs() {
});
}

if (link_head_tilt_tf) {
if (camera_color_frame_tf) {
sendData({
type: 'sensor',
subtype: 'head',
name: 'transform',
value: link_head_tilt_tf
});
}

if (jointState) {
sendData({
type: 'sensor',
subtype: 'head',
name: 'joint_transform',
value: {
pan: getJointValue(jointState, 'joint_head_pan'),
tilt: getJointValue(jointState, 'joint_head_tilt')
}
value: camera_color_frame_tf
});
}
}
Expand Down Expand Up @@ -443,12 +421,12 @@ function sendIncrementalMove(jointName, jointValueInc) {

function headLookAtGripper() {
console.log('attempting to send headLookAtGripper command')
if (link_gripper_finger_left_tf && link_head_tilt_tf) {
if (link_gripper_finger_left_tf && camera_color_frame_tf) {

var posDifference = {
x: link_gripper_finger_left_tf.translation.x - link_head_tilt_tf.translation.x,
y: link_gripper_finger_left_tf.translation.y - link_head_tilt_tf.translation.y,
z: link_gripper_finger_left_tf.translation.z - link_head_tilt_tf.translation.z
x: link_gripper_finger_left_tf.translation.x - camera_color_frame_tf.translation.x,
y: link_gripper_finger_left_tf.translation.y - camera_color_frame_tf.translation.y,
z: link_gripper_finger_left_tf.translation.z - camera_color_frame_tf.translation.z
};

// Normalize posDifference
Expand Down
23 changes: 7 additions & 16 deletions shared/sensors.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,8 +130,6 @@ var rotation_to_target_offset = global_target_point.clone().sub(global_rotation_

var headSensors = {
"transform": function (value) {
THREEcamera.position.copy(rosPostoTHREE(value.translation));
return
// 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);
Expand All @@ -154,10 +152,12 @@ var headSensors = {
var target_point = rotation_point.clone().add(rotated_rotation_offset_to_target_offset);

//THREEcamera.position.copy(target_point);
THREEcamera.position.copy(rosPostoTHREE(value.translation));

var e_three_space = rosEulerToTHREE(e, 'XYZ');
var e_three_space = rosEulerToTHREE(e, 'YZX');
THREEcamera.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);
Expand All @@ -166,16 +166,7 @@ var headSensors = {
console.log("rotation_point", rotation_point.clone().multiplyScalar(100));
console.log("target_point", target_point);
console.log("e_three_space", e_three_space);
},
"joint_transform": function (value) {
if (value.pan!==0 && value.tilt!==0) {
console.log("joint_pan", value.pan);
console.log("joint_tilt", value.tilt);

THREEcamera.rotation.x = value.tilt+Math.PI/2;
THREEcamera.rotation.z = value.pan;//+(Math.PI/2);
THREEcamera.rotation.order = 'YZX';
}
*/
}
}

Expand All @@ -185,9 +176,9 @@ function rosPostoTHREE(p) {

function rosEulerToTHREE(e, order) {
return new THREE.Euler(
0, //limitAngle(e.x-(Math.PI/2), 0, Math.PI),
e.z+(Math.PI/2),//limitAngle(e.z+(Math.PI/2), -Math.PI, Math.PI),
e.y+(Math.PI/2),//limitAngle(e.y, -Math.PI, Math.PI),
e.z+(Math.PI/2),
0,
e.y+(Math.PI/2),
order
)
}
Expand Down

0 comments on commit 363c8ca

Please sign in to comment.