quick note; fixing bug when running klampt collision-free motion planning demo code

wanted a quick collision-free motion planning demo, found klampt, went with it since there were actually working examples

ran into some errors when trying to run klamp documentation code

here are my fixes, and to replicate i include the installation instructions i used

install

http://motion.cs.illinois.edu/software/klampt/latest/pyklampt_docs/Manual-Installation.html#

pip install klampt pyopengl pyqt5 pyqtgraph pillow cvxpy
git clone https://github.com/krishauser/Klampt-examples
cd Klampt-examples/Python3/demos
python kbdrive.py ../../data/tx90roll.xml

I briefly installed from source and decided if I had to go down that route I wasn’t interested enough

install examples

git clone https://github.com/krishauser/Klampt-examples
cd Klampt-examples/Python3/demos
python kbdrive.py ../../data/tx90roll.xml

output

Python 3.10.5 (v3.10.5:f377153967, Jun  6 2022, 12:36:10) [Clang 13.0.0 (clang-1300.0.29.30)] on darwin
Type "help", "copyright", "credits" or "license" for more information.
>>> import klampt
>>> klampt.__version__
'0.9.0'

fixed code

in same folder, tried to run

cd Klampt-examples/Python3/demos
python teacup.py

the fixed code

import klampt
from klampt.plan import cspace,robotplanning
from klampt.model import trajectory
from klampt.io import resource
import time

from klampt.plan.robotcspace import RobotCSpace
from klampt.model import collide

world = klampt.WorldModel()
#world.readFile("Klampt-examples/data/tx90cuptable.xml")
success = world.readFile("../../data/tx90cuptable.xml")
if not success:
    raise RuntimeError("Unable to load world")

robot = world.robot(0)

#this is the CSpace that will be used.  Standard collision and joint limit constraints
#will be checked
#space = robotplanning.makeSpace(world,robot,edgeCheckResolution=0.05)
space = RobotCSpace(robot,collide.WorldCollider(world))

#fire up a visual editor to get some start and goal configurations
qstart = robot.getConfig()
qgoal = robot.getConfig()
save,qstart = resource.edit("Start config",qstart,"Config",world=world)
#it's worthwile to make sure that it's feasible
while save and not space.feasible(qstart):
    print("Start configuration isn't feasible, please pick one that is collision-free")
    save,qstart = resource.edit("Start config",qstart,"Config",world=world)

save,qgoal = resource.edit("Goal config",qgoal,"Config",world=world)
while save and not space.feasible(qgoal):
    print("Goal configuration isn't feasible, please pick one that is collision-free")
    save,qgoal = resource.edit("Goal config",qgoal,"Config",world=world)

#  --------------

settings = {'type':'rrt',
    'perturbationRadius':0.5,
    'bidirectional':True,
    'shortcut':True,
    'restart':True,
    'restartTermCond':"{foundSolution:1,maxIters:1000}"
}
t0 = time.time()
print("Creating planner...")
#Manual construction of planner
planner = cspace.MotionPlan(space, **settings)
planner.setEndpoints(qstart,qgoal)

print("Planner creation time",time.time()-t0)
t0 = time.time()
print("Planning...")
numIters = 0
for round in range(10):
    planner.planMore(500)
    numIters += 1
    if planner.getPath() is not None:
      break

print('\t*' * 6)
print("Planning time,",numIters,"iterations",time.time()-t0)
print('\t*' * 6)

path = planner.getPath()
if path is not None:
    print("Got a path with",len(path),"milestones")
else:
    print("No feasible path was found")



#  --------------

#provide some debugging information
V,E = planner.getRoadmap()
print(len(V),"feasible milestones sampled,",len(E),"edges connected")

print("CSpace stats:")
spacestats = space.getStats()
for k in sorted(spacestats.keys()):
    print(" ",k,":",spacestats[k])

print("Planner stats:")
planstats = planner.getStats()
for k in sorted(planstats.keys()):
    print(" ",k,":",planstats[k])

if path:
    #save planned milestone path to disk
    print("Saving to my_plan.configs")
    resource.set("my_plan.configs",path,"Configs")

    #visualize path as a Trajectory resource
    traj = trajectory.RobotTrajectory(robot,range(len(path)),path)
    resource.edit("Planned trajectory",traj,world=world)

    #Here's another way to do it: visualize path in the vis module
    from klampt import vis
    vis.add("world",world)
    vis.animate(("world",robot.getName()),path)
    vis.add("trajectory",traj)   #this draws the end effector trajectory
    vis.spin(float('inf'))

explanation

error 1. RuntimeError: Invalid robot index

The error here is actually, the path is incorrect for the world file.

#world.readFile("Klampt-examples/data/tx90cuptable.xml")
world.readFile("../../data/tx90cuptable.xml")

error 2. ValueError invalid length

The interactive window to choose a pose does pop up, but after hit “okay” immediate crashes

ValueError: Invalid length of embedded vector space vector: 12 should be 7

after digging around some other demo/test files, I believe the issues is the robot has 7 joints and the girpper has 5 joints

and there are two ways to specify a planner, one is like a higherlevel “robotplanning” and the other uses lower level “cspace” modules, and on the beginning of the page the example defines the space using robotplanning, but later define the planner using cspace.MotionPlanning.

this causes some issue i assume with loading the world consistently re: 7 vs 12 dimensions

anyway, use robotcspace

#space = robotplanning.makeSpace(world,robot,edgeCheckResolution=0.05)

space = RobotCSpace(robot,collide.WorldCollider(world))

proof

after manually picking the following points

qstart =[0.0, 0.0, 0.0, -1.3000000000000007, 0.0, 0.0, 0.0, 0.0, 0.048014007722364006, 0.048014007722364006, -0.048014007722364006, 0.048014007722364006]

qgoal = [0.0, 0.0, 0.39999999999999997, -2.18, 0.18, 0.9199999999999999, 0.0, 0.0, 0.048014007722364006, 0.048014007722364006, -0.048014007722364006, 0.048014007722364006]

then waiting 2 minutes for the motion planner (actually)

i get the following video

notes

more example code at: https://gitq.com/krishauser/Klampt

IK would be something like this

from klampt.model import ik

print(robot.numLinks())
obj = ik.objective(robot.link(robot.numLinks()-1),local=[0,0,0],world=[0.5,0,0.5])

you can also run

python gl_vis_widgets.py ../../data/tx90cuptable.xml , manually move arm,
and use menu item “print config” and the coordinates will print out on the terminal

Pandemic Diary #87 – risks (16 Sep 2022)

wow

okay a lot has happened (i saw the last date was end of july)

went home to GA (took a test 2 or 3 days in), then to a wedding (with ~100 people, almost all unmasked) – had a false positive scare too (expired test, but I did have a known exposure to someone who tested positive two or three days later; the false positive was a sickening feeling of, was I selfish to still go through with this and risk my friend attending as a bridesmaid also, followed by how logistically and expensive it would be to quarantine in NYC), then to NYC to stay with friends (ate in a restaurant without a mask on), then hung out with MITERS folks, then back to MA on a train (managed to sit next to a roboticist even). then shortly thereafter flew to Toronto, where I stayed in a hostel in a 8-bed dorm room, mostly tried to keep n95 on but didn’t manage the first night as I had the comfier but also looser one. then woke up to someone with a wet cough in the room.

so, a lot of aggressive risk taking. I had covid not too long ago and it hadn’t turned out too bad thanks to pax and a less virulent strain. that made me feel more aggressive about risk-taking. and i did miss it. i had a lot of fun eating at a restaurant with a friend and being at a party (oh shoot, need to remember to donate money in my friend’s honor, now that my paycheck is stable again)

tumultous times

school started, and somehow everything is a-ok. are there parts that could be better? certainly. but it’s not so bad, and perhaps thinking about what i could have or should be or need to be doing is detracting away from enjoying my present. it has been nice to see faces haven’t seen in a long time. actually less than i expected, but still the feeling is there. (and some movement on old projects / the force sensors, unexpected and coolllll, I guess that is a reason to publish). running services / api’s / websites for people. not just doing side projects

but honestly, after this paper process, feeling like I knew how to make a difference in the past: writing blog posts, emailing people, holding competitions or hackathons, connecting people, sponsoring others, being creative. these are all things I miss in my insecurity of publishing and finding Internships.

being able to pay it forward by treating the UROPs with respect. that has been good. (but somehow, I still haven’t asked for feedback for improving…)

need to clean up my code. but honestly something has changed. I guess I feel less despair now, like caught in an endless rabbit hole with no direction of where to go to get out. just thinking about all the wrong turns i made.

had a week straight of 2 or 3 hrs of talking about talking each day, but i fixed an area that was a huge mental energy sink. feeling my executive function come back online as school starts has been a great feeling. continuous small successes in person. interspersed with celebrating roommates leaving/coming back. a weird era for sure. the additional income is a huge energy relief also. just feeling like buying all the snacks i want and not background running calculations. the debt-free responsibility-free life (this income would not be reasonable otherwise). but harvard health insurance is actually worse in many respects. alas

made delicious dumplings with roommate who came back,

then …

roommate got sick the next day, tested positive the day after !

actually it wasn’t so stressful now that i’ve had covid before, except

wow it was a much more virulent strain, sounded awful, swallowing as razors and just too sick to even eat much

but almost felt routine, open up the windows to ventilate, set up the hepa filters, set the fans going, obsessively read about various aspects of the latest variants for a while, bring the masks and hand sanitizer out, close my own door. be paranoid but not too paranoid. test and then run chores to get cold medication and foodstuffs. order more tests. run some weird communication negotiation understanding information sharing probability calculations.

Q: ventilation != filtration

from this email newsletter i subscribe to

https://yourlocalepidemiologist.substack.com/p/a-plan-for-the-upcoming-school-year?utm_source=substack&utm_medium=email

https://sites.google.com/site/whitneyrobinsonphd/clean-air-1-pager-aug-20-2022?authuser=0

https://sites.google.com/site/whitneyrobinsonphd/clean-air-1-pager-aug-20-2022?authuser=0

(Got on a tangent trying to re-find the corsi-rosenthal diy filter, which is just box fan + two home furnace filters + duct tape)

Image source: https://en.wikipedia.org/wiki/Corsi%E2%80%93Rosenthal_Box

Q: what direction to point fans?

“When you are inside, open windows or doors whenever possible… The use of ceiling fans can improve the circulation of air from outside and avoid pockets of stagnant air forming indoors. However, it is important to bring in air from the outside by opening windows when using a ceiling fan.” https://www.who.int/news-room/questions-and-answers/item/coronavirus-disease-covid-19-ventilation-and-air-conditioning

https://www.cdc.gov/coronavirus/2019-ncov/prevent-getting-sick/Improving-Ventilation-Home.html

Image

Q: if your roommate is still testing positive when is estimate of time to them not being infectious

It looks like (on average) infectious ends around day 8 (we’re at day 9, if day 0 is Tuesday), based on attempting to culture live virus from samples

Studies using viral cultures show that, although patients can remain RNA-positive for weeks after symptom onset, live virus cannot be cultured from specimens collected later than 9 days after symptom onset, suggesting that the mean period of infectiousness and risk of transmission could be restricted to the period between 2 and 3 days before and 8 days after symptom onset. RNA-positive culture-negative samples could represent the detection of genomic fragments rather than an actively replicating virus.

Dec 2021 from https://www.thelancet.com/article/S0140-6736(21)02346-1/fulltext (also the image src) (CDC says you can def exit isolation by now (fever free 24 hrs), and can unmask around other after two negative tests 2 days apart)

Image

mostly missed hanging out in common areas and eating chips, shooting the breeze

but anyway, roommate then tested negative, so all that calculation to questionable use, but was interesting to see the latest (or at least more recent) science

but was extremely sobering to see how sick my roommate got

quals are scheduled, my advisor joked to my collaborators that i’d have a phd at the end of the semester, so … i guess time to kick my butt into gear !

Summing Table Rows by Condition with TamperMonkey Javascript

this is more just braindump notes,

we have a fancypants website for our apartment where we pay rent, so we are actually able to independently pay our parts of the rent on the website. this however led to a build up of confusion over time.

the table is structured like a balance sheet, so yuo can paste it into google docs. and the easy thing to do is to just add it up per person using the SUMIF function.

=-1 * SUMIF(B13:B110, "*Joly*", C13:C110)
=-1 * SUMIF(B13:B110, "*Brie*", C13:C110)
=-1* SUMIF(B13:B110, "*Cho*", C13:C110)
= SUMIF(C13:C110, ">0", C13:C110)

in English it is

“for each row, if column B contains the 2585 then add column C to sum”.

Oh, for the final one is, The Charges are + so sum those. (what we’ve paid is -) –> this conversion, from the $(xyz) to +/- format, is done automatically when pasted into google sheets.

querySelectorAll.forEach

find the table

The core is the function filterAndSum(). Specifically, I select all table rows that are not “hidden”. I use the “querySelectorAll” function.

        var rows = document.querySelectorAll('tr:not(.hidden)');

(Note: I noticed actually each row is repeated twice, so I had to filter to only select rows that did NOT have a “hidden” in their class.)

This returns an array which I iterate through using a for loop.

        for (let i=0; i <rows.length; i++) {

For each row in the array: If the text string in that table row contains e.g. my name,

            if (row.textContent.includes("orangenarwhals")) {

then I look for the $$$ in that row. So I use another querySelector, and look at table-data, or “td”, with class “mat-column-Amount”. In the below code, that’s what the “period” is for, it is a CSS class selctor. (see examples at https://developer.mozilla.org/en-US/docs/Web/CSS/CSS_Selectors e.g. you can specific child with “>”)

 row.querySelector('td.mat-column-Amount').textContent;

Then I used some copy-pasta to remove the $, (), and the comma from the string, e.g. $(1,234.53) turns into 1234.53

var punctuation = '$,()';
var regex = new RegExp('[' + punctuation + ']', 'g');
some_string.replace(regex, '')

Then I can use the built-in parseFloat to get a float, which I can them sum.

parseFloat

So the whole function looks like this.

function filterAndSum(str_filter) {
        var punctuation = '($,)';
        var regex = new RegExp('[' + punctuation + ']', 'g');
        //console.log(row.textContent);
        var rows = document.querySelectorAll('tr:not(.hidden)');
        var sum = 0;

        for (let i=0; i <rows.length; i++) {
            var row = rows[i];
            if (row.textContent.includes(str_filter)) {
                var str_amt = row.querySelector('td.mat-column-Amount').textContent;
                var amt = parseFloat(str_amt.replace(regex, ''));
                console.log('amount: ', amt);
                sum += amt;
            }
        }
        console.log('if filter by string', str_filter, 'there is sum', sum);
        return sum;
}

click

The other problem to solve is. That only a few months are revealed at first, so have to click “show more” button. So I can use the built-in click() to do so.

            document.querySelector('div.data-grid__show-more-container').childNodes[0].click();

This is the html

<div class="data-grid__show-more-container ng-star-inserted">
    <a bdi18n="Global.Component.Grid.ShowMore">Show more</a>
</div>

And to keep clicking until there’s no more to click..

function clickMores() {
    try{
        while (true){
            document.querySelector('div.data-grid__show-more-container').childNodes[0].click();
        }
    }
    catch (e) {
        console.log('done clicking "Show Mores"');
    }
}

Later on I added a sleep function, to allow for page load issues. This is from the internet, titled “ES6 vanilla sleep” or something like that. I guess ES6 is the latest-er version of javascript.


function sleep(ms) {
    return new Promise(resolve => setTimeout(resolve, ms));
}

Which then can be used (note “async”) like so:

async function clickMores() {
    try{
    while (true){
        document.querySelector('div.data-grid__show-more-container').childNodes[0].click();
        await sleep(1000);
    }

Show the information: make a div

then for the lulz I wanted to show the information. So create a div. Add text. Style. Figure out where to put it.

    var my_div = document.createElement('div');
    my_div.innerHTML = 'Please wait 10 seconds for the magic to happen :)';
    my_div.setAttribute("style", "font-size: 35px; color:red;");
    document.querySelector('body').prepend(my_div);

Later we can clean up this div

my_div.setAttribute('style', "visibility: hidden;");

templates with backticks

so i think python might have copied this from javascript… you can put expressions inside a string. just use backticks instead of quotes.

my_info_div.innerHTML = `Cool Info: Sum for N is ${filterAndSum('orangenarwhals')}`

all together

// ==UserScript==
// @name         New Userscript
// @namespace    http://tampermonkey.net/
// @version      0.1
// @description  try to take over the world!
// @author       You
// @match        https://bradleyproperties.managebuilding.com/Resident/portal/payments
// @icon         https://www.google.com/s2/favicons?sz=64&domain=managebuilding.com
// @grant        none
// ==/UserScript==


async function clickMores() {
    console.log('trying to click');
    try{
        while (true){
            console.log('clicking');
            document.querySelector('div.data-grid__show-more-container').childNodes[0].click();
            await sleep(1000);
        }
    }
    catch (e) {
        console.log('done clicking "Show Mores"');
    }

}

function filterAndSum(str_filter) {
        var punctuation = '($,)';
        var regex = new RegExp('[' + punctuation + ']', 'g');
        //console.log(row.textContent);
        var rows = document.querySelectorAll('tr:not(.hidden)');
        var sum = 0;

        for (let i=0; i <rows.length; i++) {
            var row = rows[i];
            if (row.textContent.includes(str_filter)) {
                //console.log('found string', row.textContent);
                var str_amt = row.querySelector('td.mat-column-Amount').textContent;
                var amt = parseFloat(str_amt.replace(regex, ''));
                console.log('amount: ', amt);
                sum += amt;
            }
        }
        console.log('if filter by string', str_filter, 'there is sum', sum);
        return sum;
}


function sleep(ms) {
    return new Promise(resolve => setTimeout(resolve, ms));
}

(async function() {
    console.log('hi');
    'use strict';

    var my_div = document.createElement('div');
    my_div.innerHTML = 'Please wait 10 seconds for the magic to happen :)';
    my_div.setAttribute("style", "font-size: 35px; color:red;");
    document.querySelector('body').prepend(my_div);


    console.log('Taking an 4 sec break...');
    await sleep(4000);
    console.log('4 seconds later...');


    my_div.innerHTML = 'Please wait for the magic to happen, we are clicking the page for you :)';
    my_div.setAttribute("style", "font-size: 35px; color:red;");
    document.querySelector('table').parentElement.prepend(my_div);


    clickMores();

    await sleep(2500); // i guess must wait for the clicks if they're asynchronous?


    var charges = filterAndSum('Charge');

    var expect_A = charges / 3.5 * .5;
    var expect_B = charges / 3.5; 
    var expect_C = charges / 3.5 * 2;

    var AAA = filterAndSum('nameA');
    var BBB = filterAndSum('nameB');
    var CCC = filterAndSum('nameC');

    var my_info_div = document.createElement('div');

    my_info_div.innerHTML = `Cool Info:
<br>  Sum for A is ${AAA}
<br>  Sum for B is ${BBB}
<br>  Sum for C is ${CCC}
<br>
<br>  Total Charges are ${charges}
<br>  Total Paid is  ${AAA+BBB+CCC}
<br>  Diff is ${charges - (AAA+BBB+CCC)}
<br>  ============================
<br>  Approx. Expectations (note that we are rounding to cents so won't quite match),
<br>
<br>  A: ${expect_A}, Diff is ${expect_A - AAA}, aka ${Math.round(expect_A - AAA)}
<br>  B: ${expect_B}, Diff is ${expect_B - BBB}, aka ${Math.round(expect_B - BBB)}
<br>  C: ${expect_C}, Diff is ${expect_C - CCC}, aka ${Math.round(expect_C - CCC)}
  `;
    my_info_div.setAttribute("style", "font-size: 35px; color:blue;");
    document.querySelector('table').parentElement.prepend(my_info_div);
    my_div.setAttribute('style', "visibility: hidden;");

}

Misc. Notes

IKEA - get all URLs on shopping cart
document.querySelectorAll("[data-testid=product_name]").forEach(function getHref(result){
console.log(`${result.childNodes[0].href}`);
});

XPath

    xpath=//div[contains(text(), "orangenarwhals")]/..//following-sibling::td[contains(@class, "mat-column-Amount")]/b/text()
    amts_iterator = document.evaluate(xpath, document.body, null, XPathResult.ANY_TYPE,
            null);

    var amt = amts_iterator.iterateNext();

    while (amt) {
        sum += blahblah(amt);
        amt = amts_iterator.iterateNext();
    };

http://ponderer.org/download/xpath/

https://scrapfly.io/blog/parsing-html-with-xpath/

https://jsfiddle.net/34wf5gzs/