-
Notifications
You must be signed in to change notification settings - Fork 17
/
tools.py
110 lines (86 loc) · 4.41 KB
/
tools.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Wed Sep 6 15:32:51 2023
@author: stonneau
"""
import pinocchio as pin #the pinocchio library
import numpy as np
def jointlimitscost(robot,q):
up = max(q - robot.model.upperPositionLimit)
down = max(robot.model.lowerPositionLimit - q)
return max(0,max(up,down))
def jointlimitsviolated(robot,q):
'''Return true if config not in joint limits'''
return jointlimitscost(robot,q) > 0.
def projecttojointlimits(robot,q):
return np.minimum(np.maximum(robot.model.lowerPositionLimit, q), robot.model.upperPositionLimit)
def collision(robot, q):
'''Return true if in collision, false otherwise.'''
pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)
# if pin.computeCollisions(robot.collision_model,robot.collision_data,False):
# for k in range(len(robot.collision_model.collisionPairs)):
# cr = robot.collision_data.collisionResults[k]
# cp = robot.collision_model.collisionPairs[k]
# if cr.isCollision():
# print("collision pair:",robot.collision_model.geometryObjects[cp.first].name,",",robot.collision_model.geometryObjects[cp.second].name,"- collision:","Yes" if cr.isCollision() else "No")
return pin.computeCollisions(robot.collision_model,robot.collision_data,False)
def distanceToObstacle(robot, q):
'''Return the shortest distance between robot and the obstacle. '''
geomidobs = robot.collision_model.getGeometryId('obstaclebase_0')
geomidtable = robot.collision_model.getGeometryId('baseLink_0')
pairs = [i for i, pair in enumerate(robot.collision_model.collisionPairs) if pair.second == geomidobs or pair.second == geomidtable]
pin.framesForwardKinematics(robot.model,robot.data,q)
pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)
dists = [pin.computeDistance(robot.collision_model, robot.collision_data, idx).min_distance for idx in pairs]
# pairsId = [pair.first for i, pair in enumerate(robot.collision_model.collisionPairs) if pair.second == geomidobs or pair.second == geomidtable]
# names = [robot.collision_model.geometryObjects[idx].name for idx in pairsId ]
# for name, dist in zip(names,dists):
# print ("name / distance ", name, " / ", dist)
# print(min (dists))
return min(dists)
def getcubeplacement(cube, hookname = None):
oMf = cube.collision_model.geometryObjects[0].placement
if hookname is not None:
frameid = cube.model.getFrameId(hookname)
oMf *= cube.data.oMf[frameid]
return oMf
def setcubeplacement(robot, cube, oMf):
q = cube.q0
robot.visual_model.geometryObjects[-1].placement = oMf
robot.collision_model.geometryObjects[-1].placement = oMf
cube.visual_model.geometryObjects[-1].placement = oMf
cube.collision_model.geometryObjects[0].placement = oMf
pin.updateGeometryPlacements(cube.model,cube.data,cube.collision_model,cube.collision_data,q)
from setup_pinocchio import setuppinocchio
from setup_meshcat import setupmeshcat
from config import MESHCAT_URL
def setupwithmeshcat(url=MESHCAT_URL):
'''setups everything to work with the robot and meshcat'''
robot, table, obstacle, cube = setuppinocchio()
viz = setupmeshcat(robot)
return robot, cube, viz
from setup_pybullet import setuppybullet
def setupwithpybullet():
'''setups everything to work with the robot and pybullet'''
robot, table, obstacle, cube = setuppinocchio()
sim = setuppybullet(robot)
sim.setTorqueControlMode()
return robot, sim, cube
def setupwithpybulletandmeshcat(url=MESHCAT_URL):
'''setups everything to work with the robot, pybullet AND meshcat'''
robot, table, obstacle, cube = setuppinocchio()
viz = setupmeshcat(robot)
sim = setuppybullet(robot)
sim.setTorqueControlMode()
return robot, sim, cube, viz
import time
def rununtil(f, t, *args, **kwargs):
'''starts a timer, runs a function f then waits until t seconds have passed since timer started'''
t = time.perf_counter()
# Call the provided function f2 with its arguments
result = f(*args, **kwargs)
t+=0.001
while(time.perf_counter()-t<0.001):
time.sleep(0.0001) # Weird loop for parallel execution (should not be needed in this project)
return result