Skip to content

Commit

Permalink
Merge pull request #28 from hcrlab/#11-decrease-manipulation-burden
Browse files Browse the repository at this point in the history 
#11 decrease manipulation burden
  • Loading branch information
@mayacakmak
mayacakmak authored Jun 26, 2021
2 parents acd3154 + 87fd973 commit 815dbe7
Show file tree
Hide file tree
Showing 5 changed files with 223 additions and 50 deletions.
8 changes: 8 additions & 0 deletions operator/operator.css
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
height: 620px;
width: 350px;
z-index:0;
background-color: gray;
}

#video_ui_overlay {
Expand Down Expand Up @@ -75,6 +76,13 @@
cursor: url('right_turn_medium.png'), auto;
}

#nav_ccw_region {
cursor: url('left_turn_medium.png'), auto;
}

#nav_cw_region {
cursor: url('right_turn_medium.png'), auto;
}

#low_arm_down_region {
cursor: url('down_arrow_medium.png'), auto;
Expand Down
114 changes: 68 additions & 46 deletions operator/operator.html
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,59 +83,81 @@
<svg id="video_ui_overlay">

<svg id="nav_ui_overlay" preserveAspectRatio="none">
<path
fill-opacity="0.0"
stroke-opacity="1.0"
id="nav_do_nothing_region"
>
<title>do nothing</title>
</path>
<path
fill-opacity="0.0"
stroke-opacity="1.0"
id="nav_do_nothing_region"
>
<title>do nothing</title>
</path>

<!--
<svg id="right_arrow" version="1.1" viewBox="0.0 0.0 37.79527559055118 37.79527559055118" fill="none" stroke="none" stroke-linecap="square" stroke-miterlimit="10" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink"><clipPath id="p.0"><path d="m0 0l37.795277 0l0 37.795277l-37.795277 0l0 -37.795277z" clip-rule="nonzero"></path></clipPath><g clip-path="url(#p.0)"><path fill="#000000" fill-opacity="0.0" d="m0 0l37.795277 0l0 37.795277l-37.795277 0z" fill-rule="evenodd"></path><path fill="#ffffff" d="m1.4330709 15.178587l21.404064 0l0 -6.3911858l13.52507 10.110237l-13.52507 10.110235l0 -6.3911858l-21.404064 0z" fill-rule="evenodd"></path><path stroke="#000000" stroke-width="2.0" stroke-linejoin="round" stroke-linecap="butt" d="m1.4330709 15.178587l21.404064 0l0 -6.3911858l13.52507 10.110237l-13.52507 10.110235l0 -6.3911858l-21.404064 0z" fill-rule="evenodd"></path></g></svg>
-->

<path
fill-opacity="0.0"
stroke-opacity="1.0"
id="nav_forward_region"
onclick="moveForward()"
onmousedown="startAction('moveForward')"
>
<title>move forward</title>
</path>

<path
fill-opacity="0.0"
stroke-opacity="1.0"
id="nav_backward_region"
onclick="moveBackward()"
onmousedown="startAction('moveBackward')"
>
<title>move backward</title>
</path>

<path
fill-opacity="0.0"
stroke-opacity="1.0"
id="nav_turn_left_region"
onclick="turnLeft()"
onmousedown="startAction('turnLeft')"
>
<title>turn left</title>
</path>
<path
fill-opacity="0.0"
stroke-opacity="1.0"
id="nav_forward_region"
onclick="moveForward()"
onmousedown="startAction('moveForward')"
>
<title>move forward</title>
</path>

<path
fill-opacity="0.0"
stroke-opacity="1.0"
id="nav_backward_region"
onclick="moveBackward()"
onmousedown="startAction('moveBackward')"
>
<title>move backward</title>
</path>

<path
fill-opacity="0.0"
stroke-opacity="1.0"
id="nav_turn_left_region"
onclick="turnLeft()"
onmousedown="startAction('turnLeft')"
>
<title>turn left</title>
</path>

<path
fill-opacity="0.0"
stroke-opacity="1.0"
id="nav_turn_right_region"
onclick="turnRight()"
onmousedown="startAction('turnRight')"
>
<title>turn right</title>
</path>

<path
fill-opacity="0.0"
stroke-opacity="1.0"
id="nav_turn_right_region"
onclick="turnRight()"
onmousedown="startAction('turnRight')"
>
<title>turn right</title>
</path>

<g
id="ccw_cw_text_region"
>
</g>
<path
fill-opacity="0.0"
stroke-opacity="1.0"
id="nav_ccw_region"
onclick="turnCCW()"
onmousedown="startAction('turnCCW')"
>
<title>turn ccw 90</title>
</path>
<path
fill-opacity="0.0"
stroke-opacity="1.0"
id="nav_cw_region"
onclick="turnCW()"
onmousedown="startAction('turnCW')"
>
<title>turn cw 90</title>
</path>
</svg>


Expand Down
33 changes: 29 additions & 4 deletions operator/operator_ui_regions.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,24 @@ function rectToPoly(rect) {
return [rect.ul, rect.ur, rect.lr, rect.ll];
}

function drawText(elementID, text, x, y, font_size=100, center=false, color='white') {
var txt = document.createElementNS("http://www.w3.org/2000/svg", 'text');
txt.setAttributeNS(null, 'x', x);
txt.setAttributeNS(null, 'y', y);
txt.setAttributeNS(null, 'font-size', String(font_size));
txt.setAttributeNS(null, 'fill', color);
txt.setAttributeNS(null, 'stroke', 'black');
txt.setAttributeNS(null, 'stroke-width', '5');
txt.setAttributeNS(null, 'paint-order', 'stroke')
if (center) {
txt.setAttributeNS(null, 'text-anchor', 'middle')
txt.setAttributeNS(null, 'alignment-baseline', 'middle')
}

txt.innerHTML = text;
document.getElementById(elementID).appendChild(txt);
}

function hideSvg(elementId) {
document.getElementById(elementId).style.display = 'none';
}
Expand Down Expand Up @@ -75,12 +93,12 @@ function createUiRegions(debug) {
strokeOpacity = 0.0;
}

function setRegionPoly(elementId, poly, color) {
function setRegionPoly(elementId, poly, color, stroke_width = 2, stroke_opacity = false) {
var region = document.getElementById(elementId);
region.setAttribute('stroke', color);
region.setAttribute('stroke-opacity', String(strokeOpacity));
region.setAttribute('stroke-opacity', String(stroke_opacity ? stroke_opacity : strokeOpacity));
region.setAttribute('stroke-linejoin', "round");
region.setAttribute('stroke-width', "2");
region.setAttribute('stroke-width', String(stroke_width));

region.setAttribute('d', svgPolyString(poly));
}
Expand Down Expand Up @@ -125,6 +143,7 @@ function createUiRegions(debug) {
regionPoly = [bgRect.ul, smRect.ul, smRect.ll, bgRect.ll];
setRegionPoly('nav_turn_left_region', regionPoly, color);


//var region = document.getElementById('right_arrow')
//region.setAttribute('stroke-opacity', "0.1");
//region.setAttribute('fill-opacity', "0.1");
Expand All @@ -137,7 +156,13 @@ function createUiRegions(debug) {
regionPoly = [bgRect.ur, smRect.ur, smRect.lr, bgRect.lr];
setRegionPoly('nav_turn_right_region', regionPoly, color);

navModeRegionIds = ['nav_do_nothing_region', 'nav_forward_region', 'nav_backward_region', 'nav_turn_left_region', 'nav_turn_right_region']
var size = 80;
setRegionPoly('nav_ccw_region', rectToPoly(makeSquare(0, h-size, size)), color, 2, 0.5);
drawText('ccw_cw_text_region','⤹ 90°', size/2, h-size+size/1.5, 25, true)
setRegionPoly('nav_cw_region', rectToPoly(makeSquare(w-size, h-size, size)), color, 2, 0.5);
drawText('ccw_cw_text_region','90° ⤸', w-size+size/2, h-size+size/1.5, 25, true)

navModeRegionIds = ['nav_do_nothing_region', 'nav_forward_region', 'nav_backward_region', 'nav_turn_left_region', 'nav_turn_right_region', 'nav_ccw_region', 'nav_cw_region']


///////////////////////
Expand Down
90 changes: 90 additions & 0 deletions robot/ros_connect.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,11 @@ tfClient.subscribe('link_head_tilt', function(tf) {
link_head_tilt_tf = tf;
});

var base_tf;
tfClient.subscribe('odom', function(tf) {
base_tf = tf;
});

var trajectoryClients = {}
trajectoryClients.main = new ROSLIB.ActionClient({
ros : ros,
Expand Down Expand Up @@ -168,6 +173,7 @@ function generatePoseGoal(pose){
jointNames.push(key)
jointPositions.push(pose[key])
}

if (!inSim) {
var t = trajectoryClients.main;
} else {
Expand Down Expand Up @@ -323,6 +329,9 @@ function baseTranslate(dist, vel) {
// velocity in centimeters / second
console.log('sending baseTranslate command')

// stop trying to turn to a target if the user attempts to to move the robot base
interruptTurn = true;

if (dist > 0.0){
var baseForwardPoseGoal = generatePoseGoal({'translate_mobile_base': -vel})
baseForwardPoseGoal.send()
Expand All @@ -331,13 +340,17 @@ function baseTranslate(dist, vel) {
baseBackwardPoseGoal.send()
}
//sendCommandBody({type: "base",action:"translate", dist:dist, vel:vel});

}

function baseTurn(ang_deg, vel) {
// angle in degrees
// velocity in centimeter / second (linear wheel velocity - same as BaseTranslate)
console.log('sending baseTurn command')

// stop trying to turn to a target if the user attempts to to move the robot base
interruptTurn = true;

if (ang_deg > 0.0){
var baseTurnLeftPoseGoal = generatePoseGoal({'rotate_mobile_base': -vel})
baseTurnLeftPoseGoal.send()
Expand All @@ -348,6 +361,41 @@ function baseTurn(ang_deg, vel) {
//sendCommandBody({type: "base",action:"turn", ang:ang_deg, vel:vel});
}

var targetAngle;
var interruptTurn = false;
var turnLoop;
function turnAngleOffset(offset) {
console.log(offset)
targetAngle = quaternionToEuler(base_tf.rotation).z + offset; // this needs to be limited from 180 to -180
targetAngle = limitAngle(targetAngle)

interruptTurn = false;

turnLoop = setInterval(function() {
var error = targetAngle-quaternionToEuler(base_tf.rotation).z;

if (!interruptTurn && Math.abs(error) > 0.05) {
//console.log(targetAngle, quaternionToEuler(base_tf.rotation).z, error);
generatePoseGoal({'rotate_mobile_base': error/10}) // super simple proportional control
} else {
clearInterval(turnLoop);

}

}, 200);
}

function limitAngle(rad) {
while (rad > Math.PI/2) {
rad -= Math.PI;
}
while (rad < -Math.PI/2) {
rad += Math.PI;
}

return rad;
}

function getJointValue(jointStateMessage, jointName) {
if (inSim && jointName == 'wrist_extension') {
var value = 0;
Expand Down Expand Up @@ -535,4 +583,46 @@ function gripperHalfOpen() {
function gripperFullyOpen() {
console.log('sending fully open gripper command');
sendCommandWrist({type:'gripper', action:'fully_open'});
}

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

// Modified from https://schteppe.github.io/cannon.js/docs/files/src_math_Quaternion.js.html
function quaternionToEuler(q, order){
order = order || "YZX";

var heading, attitude, bank;
var x = q.x, y = q.y, z = q.z, w = q.w;

switch(order){
case "YZX":
var test = x*y + z*w;
if (test > 0.499) { // singularity at north pole
heading = 2 * Math.atan2(x,w);
attitude = Math.PI/2;
bank = 0;
}
if (test < -0.499) { // singularity at south pole
heading = -2 * Math.atan2(x,w);
attitude = - Math.PI/2;
bank = 0;
}
if(isNaN(heading)){
var sqx = x*x;
var sqy = y*y;
var sqz = z*z;
heading = Math.atan2(2*y*w - 2*x*z , 1 - 2*sqy - 2*sqz); // Heading
attitude = Math.asin(2*test); // attitude
bank = Math.atan2(2*x*w - 2*y*z , 1 - 2*sqx - 2*sqz); // bank
}
break;
default:
throw new Error("Euler order "+order+" not supported yet.");
}

return {
y: heading,
z: attitude,
x: bank
}
}
28 changes: 28 additions & 0 deletions shared/commands.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,24 @@ function turnRight() {
Database.logEvent("TurnRight", currentV);
}

function turnCCW() {
var cmd = {type:"command",
subtype:"drive",
name:"turn_ccw",
modifier:"none"};
sendData(cmd);
Database.logEvent("TurnCCW", currentV);
}

function turnCW() {
var cmd = {type:"command",
subtype:"drive",
name:"turn_cw",
modifier:"none"};
sendData(cmd);
Database.logEvent("TurnCW", currentV);
}

function liftUp() {
var cmd = {type:"command",
subtype:"lift",
Expand Down Expand Up @@ -500,6 +518,16 @@ var driveCommands = {
// executeCommandBySize(size, baseTurn,
// [-1.0, 300.0], // angle (deg), angular speed (deg/s)
// [-10.0, 300.0]); // angle (deg), angular speed (deg/s)
},
"turn_ccw": function(size) {
console.log('drive: turn_ccw command received...executing');

turnAngleOffset(Math.PI/2); // 90 deg
},
"turn_cw": function(size) {
console.log('drive: turn_cw command received...executing');

turnAngleOffset(-Math.PI/2); // -90 deg
}
}

Expand Down

0 comments on commit 815dbe7

Please sign in to comment.