From 25386fb7a5faaee6af3f7890af18518e4bc256f0 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 2 Dec 2024 00:03:27 +0100
Subject: [PATCH] i managed to break path generation while porting. this is
priority number 1. other than that, there are at least 3 very evil things
that need to be addressed to make the code not evil
---
python/examples/cart_pulling.py | 29 +++-
python/ur_simple_control/clik/clik.py | 78 ++++++++++-
.../optimal_control/crocoddyl_mpc.py | 118 +----------------
.../path_generation/planner.py | 125 ++++++++++++++++--
.../__pycache__/visualize.cpython-312.pyc | Bin 16632 -> 16871 bytes
.../meshcat_viewer_wrapper/visualizer.py | 17 ++-
.../ur_simple_control/visualize/visualize.py | 6 +
7 files changed, 242 insertions(+), 131 deletions(-)
diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 784d5d3..e702cee 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -10,11 +10,11 @@ from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoPathFollowingMP
from ur_simple_control.basics.basics import followKinematicJointTrajP
from ur_simple_control.util.logging_utils import LogManager
from ur_simple_control.visualize.visualize import plotFromDict
-from ur_simple_control.clik.clik import getClikArgs
+from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner
import pinocchio as pin
import crocoddyl
import importlib.util
-from ur_simple_control.path_generation.planner import starPlanner, getPlanningArgs
+from ur_simple_control.path_generation.planner import starPlanner, getPlanningArgs, createMap
import yaml
@@ -44,14 +44,37 @@ if __name__ == "__main__":
if importlib.util.find_spec('mim_solvers'):
import mim_solvers
robot = RobotManager(args)
+ robot.q[0] = 9.0
+ robot.q[1] = 4.0
#time.sleep(5)
x0 = np.concatenate([robot.getQ(), robot.getQd()])
robot._step()
goal = np.array([0.5, 5.5])
+
+ ###########################
+ # visualizing obstacles #
+ ###########################
+ _, map_as_list = createMap()
+ # we're assuming rectangles here
+ # be my guest and implement other options
+ if args.visualize_manipulator:
+ for obstacle in map_as_list:
+ length = obstacle[1][0] - obstacle[0][0]
+ width = obstacle[3][1] - obstacle[0][1]
+ height = 0.4 # doesn't matter because plan because planning is 2D
+ pose = pin.SE3(np.eye(3), np.array([
+ obstacle[0][0] + (obstacle[1][0] - obstacle[0][0]) / 2,
+ obstacle[0][1] + (obstacle[3][1] - obstacle[0][1]) / 2,
+ 0.0]))
+ dims = [length, width, height]
+ command = {"obstacle" : [pose, dims]}
+ robot.visualizer_manager.sendCommand(command)
+
planning_function = partial(starPlanner, goal)
path_planner = ProcessManager(args, planning_function, None, 2, None)
- CrocoPathFollowingMPC(args, robot, x0, path_planner)
+ cartesianPathFollowingWithPlanner(args, robot, path_planner)
+ #CrocoPathFollowingMPC(args, robot, x0, path_planner)
print("final position:")
print(robot.getT_w_e())
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 40511b8..3255f18 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -3,10 +3,11 @@ import numpy as np
import copy
import argparse
from functools import partial
-from ur_simple_control.managers import ControlLoopManager, RobotManager
+from ur_simple_control.managers import ControlLoopManager, RobotManager, ProcessManager
import time
from qpsolvers import solve_qp
import argparse
+from ur_simple_control.path_generation.planner import path2D_to_timed_SE3, pathPointFromPathParam
def getClikArgs(parser):
"""
@@ -85,7 +86,7 @@ def jacobianTranspose(J, err_vector):
qd = J.T @ err_vector
return qd
-def invKinmQP(J, err_vector):
+def invKinmQP(J, err_vector, lb=None, ub=None):
# maybe a lower precision dtype is equally good, but faster?
P = np.eye(J.shape[1], dtype="double")
# TODO: why is q all 0?
@@ -95,8 +96,8 @@ def invKinmQP(J, err_vector):
b = err_vector#[:3]
A = J#[:3]
# TODO: you probably want limits here
- lb = None
- ub = None
+ #lb = None
+ #ub = None
h = None
#qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
@@ -258,9 +259,72 @@ def moveLFollowingLine(args, robot, goal_point):
"""
pass
-# TODO: implement
-def cartesianPathFollowing(args, robot, path):
- pass
+def cartesianPathFollowingWithPlannerControlLoop(args, robot : RobotManager, path_planner : ProcessManager, i, past_data):
+ """
+ cartesianPathFollowingWithPlanner
+ -----------------------------
+ end-effector(s) follow their path(s) according to what a 2D path-planner spits out
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_dict = {}
+
+ q = robot.getQ()
+ T_w_e = robot.getT_w_e()
+ p = q[:2]
+ path_planner.sendFreshestCommand({'p' : p})
+
+ # NOTE: it's pointless to recalculate the path every time
+ # if it's the same 2D path
+ data = path_planner.getData()
+ if data == None:
+ if args.debug_prints:
+ print("got no path so no doing anything")
+ robot.sendQd(np.zeros(robot.model.nv))
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+ if data == "done":
+ breakFlag = True
+
+ path_pol, path2D_untimed = data
+ path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
+ # should be precomputed somewhere but this is nowhere near the biggest problem
+ max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
+
+ # 1) make 6D path out of [[x0,y0],...]
+ # that represents the center of the cart
+ pathSE3 = path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v)
+ #print(path2D_untimed)
+ #for pp in pathSE3:
+ # print(pp.translation)
+ # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
+ if args.visualize_manipulator:
+ robot.visualizer_manager.sendCommand({"path": pathSE3})
+
+ J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
+ # NOTE: obviously not path following but i want to see things working here
+ SEerror = T_w_e.actInv(pathSE3[1])
+ err_vector = pin.log6(SEerror).vector
+ #vel_cmd = invKinmQP(J, err_vector, lb=-1*robot.model.velocityLimit,
+ # ub=robot.model.velocityLimit)
+ #vel_cmd = invKinmQP(J, err_vector)
+ vel_cmd = dampedPseudoinverse(0.002, J, err_vector)
+ robot.sendQd(vel_cmd)
+
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ return breakFlag, save_past_dict, log_item
+
+def cartesianPathFollowingWithPlanner(args, robot : RobotManager, path_planner : ProcessManager):
+ controlLoop = partial(cartesianPathFollowingWithPlannerControlLoop, args, robot, path_planner)
+ log_item = {
+ 'qs' : np.zeros(robot.model.nq),
+ 'dqs' : np.zeros(robot.model.nv)
+ }
+ save_past_dict = {}
+ loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+ loop_manager.run()
def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
"""
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index b012566..c85e7da 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -1,5 +1,6 @@
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager, ProcessManager
from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP, createCrocoPathFollowingOCP1
+from ur_simple_control.path_generation.planner import path2D_to_timed_SE3, pathPointFromPathParam
import pinocchio as pin
import crocoddyl
import numpy as np
@@ -93,118 +94,6 @@ def CrocoIKMPC(args, robot, x0, goal):
loop_manager.run()
-def pathPointFromPathParam(n_pol, path_dim, path_pol, s):
- return [np.polyval(path_pol[j*(n_pol+1):(j+1)*(n_pol+1)], s) for j in range(path_dim)]
-
-def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v):
- """
- path2D_to_timed_SE3
- ---------------------
- we have a 2D path of (N,2) shape as reference
- the OCP accepts SE3 (it could just rotations or translation too),
- so this function constructs it from the 2D path.
- it also times it as this is what the current ocp formulation needs:
- there should be a timing associated with the path,
- because defining the cost function as the fit of the rollout to the path
- is complicated to do software-wise.
- as it is now, we're pre-selecting points of the path, and associating those
- with rollout states at a set time step between the states in the rollout.
- this isn't a problem if we have an idea of how fast the robot can go,
- which gives us a heuristic of how to select the points (if you went at maximum
- speed, this is how far you'd go in this time, so this is your point).
- thankfully this does not need to be exact because we just care about the distance
- between the current point and the point on the path, so if it's further out
- that just means the error is larger, and that doesn't necessarily correspond
- to a different action.
- NOTE: we are constructing a possibly bullshit
- trajectory here, it's a man-made heuristic,
- and we should leave that to the MPC,
- but that requires too much coding and there is no time rn.
- """
-
- ####################################################
- # getting a timed 2D trajectory from a heuristic #
- ####################################################
- # the strategy is somewhat reasonable at least:
- # assume we're moving at 90% max velocity in the base,
- # and use that.
- perc_of_max_v = 0.9
- base_v = perc_of_max_v * max_base_v
-
- # 1) the length of the path divided by 0.9 * max_vel
- # gives us the total time of the trajectory,
- # so we first compute that
- # easiest possible way to get approximate path length
- # (yes it should be from the polynomial approximation but that takes time to write)
- x_i = path2D_untimed[:,0][:-1] # no last element
- y_i = path2D_untimed[:,1][:-1] # no last element
- x_i_plus_1 = path2D_untimed[:,0][1:] # no first element
- y_i_plus_1 = path2D_untimed[:,1][1:] # no first element
- x_diff = x_i_plus_1 - x_i
- x_diff = x_diff.reshape((-1,1))
- y_diff = y_i_plus_1 - y_i
- y_diff = y_diff.reshape((-1,1))
- path_length = np.sum(np.linalg.norm(np.hstack((x_diff, y_diff)), axis=1))
- total_time = path_length / base_v
- # 2) we find the correspondence between s and time
- # TODO: read from where it should be, but seba checked that it's 5 for some reason
- max_s = 5
- t_to_s = max_s / total_time
- # 3) construct the ocp-timed 2D path
- path2D = []
- # time is i * args.ocp_dt
- for i in range(args.n_knots + 1):
- s = (i * args.ocp_dt) * t_to_s
- path2D.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
- path2D = np.array(path2D)
-
- ####################################################
- # constructing the SE3 reference from [x,y] path #
- ####################################################
- # 1) we need to add points so that there are args.n_knots of them
- # the -1 is here because we're finite differencing to get theta,
- # so we need one extra
- #path2D = list(path2D)
- ## in case there's too few
- #while len(path2D) - 1 < args.n_knots:
- # path2D.append(path2D[-1])
- ## in case there's too much
- #while len(path2D) - 1> args.n_knots:
- # path2D.pop()
- #path2D = np.array(path2D)
-
-
- #########################
- # path2D into pathSE2 #
- #########################
- # construct theta
- # since it's a pairwise operation it can't be done on the last point
- x_i = path2D[:,0][:-1] # no last element
- y_i = path2D[:,1][:-1] # no last element
- x_i_plus_1 = path2D[:,0][1:] # no first element
- y_i_plus_1 = path2D[:,1][1:] # no first element
- x_diff = x_i_plus_1 - x_i
- y_diff = y_i_plus_1 - y_i
- # elementwise arctan2
- thetas = np.arctan2(x_diff, y_diff)
-
- #thetas = thetas.reshape((-1, 1))
- #path_SE2 = np.hstack((path2D, thetas))
-
- ######################################
- # construct SE3 from SE2 reference #
- ######################################
- # the plan is parallel to the ground because it's a mobile
- # manipulation task
- # TODO: enforce timing, interpolate the path accordingly
- path = []
- for i in range(len(path2D) - 1):
- rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
- translation = np.array([path2D[i][0], path2D[i][1],
- args.handlebar_height])
- path.append(pin.SE3(rotation, translation))
-
- return path
def CrocoPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocoddyl.SolverBoxFDDP, path_planner : ProcessManager, i, past_data):
"""
@@ -230,6 +119,8 @@ def CrocoPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocod
log_item['qs'] = q.reshape((robot.model.nq,))
log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
return breakFlag, save_past_dict, log_item
+ if data == "done":
+ breakFlag = True
path_pol, path2D_untimed = data
path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
@@ -239,6 +130,9 @@ def CrocoPathFollowingMPCControlLoop(args, robot : RobotManager, solver : crocod
# 1) make 6D path out of [[x0,y0],...]
# that represents the center of the cart
pathSE3 = path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v)
+ # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
+ if args.visualize_manipulator:
+ robot.visualizer_manager.sendCommand({"path": pathSE3})
# 2) do T_mobile_base_ee_pos to get
# end-effector reference frame(s)
diff --git a/python/ur_simple_control/path_generation/planner.py b/python/ur_simple_control/path_generation/planner.py
index 655d815..9596d3c 100644
--- a/python/ur_simple_control/path_generation/planner.py
+++ b/python/ur_simple_control/path_generation/planner.py
@@ -9,6 +9,7 @@ from ur_simple_control.path_generation.star_navigation.robot.unicycle import Uni
from ur_simple_control.path_generation.starworlds.obstacles import StarshapedPolygon
import shapely
import yaml
+import pinocchio as pin
import matplotlib.pyplot as plt
import matplotlib.collections as plt_col
@@ -396,6 +397,7 @@ def createMap():
---------
return obstacles that define the 2D map
"""
+ # [lower_left, lower_right, top_right, top_left]
map_as_list = [
[[2, 2], [8, 2], [8, 3], [2, 3]] ,
[[2, 3], [3, 3], [3, 4.25], [2, 4.25]],
@@ -457,6 +459,119 @@ def pathVisualizer(x0, goal, map_as_list, positions):
# return path_SE2
+def pathPointFromPathParam(n_pol, path_dim, path_pol, s):
+ return [np.polyval(path_pol[j*(n_pol+1):(j+1)*(n_pol+1)], s) for j in range(path_dim)]
+
+def path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v):
+ """
+ path2D_to_timed_SE3
+ ---------------------
+ we have a 2D path of (N,2) shape as reference
+ the OCP accepts SE3 (it could just rotations or translation too),
+ so this function constructs it from the 2D path.
+ it also times it as this is what the current ocp formulation needs:
+ there should be a timing associated with the path,
+ because defining the cost function as the fit of the rollout to the path
+ is complicated to do software-wise.
+ as it is now, we're pre-selecting points of the path, and associating those
+ with rollout states at a set time step between the states in the rollout.
+ this isn't a problem if we have an idea of how fast the robot can go,
+ which gives us a heuristic of how to select the points (if you went at maximum
+ speed, this is how far you'd go in this time, so this is your point).
+ thankfully this does not need to be exact because we just care about the distance
+ between the current point and the point on the path, so if it's further out
+ that just means the error is larger, and that doesn't necessarily correspond
+ to a different action.
+ NOTE: we are constructing a possibly bullshit
+ trajectory here, it's a man-made heuristic,
+ and we should leave that to the MPC,
+ but that requires too much coding and there is no time rn.
+ """
+
+ ####################################################
+ # getting a timed 2D trajectory from a heuristic #
+ ####################################################
+ # the strategy is somewhat reasonable at least:
+ # assume we're moving at 90% max velocity in the base,
+ # and use that.
+ perc_of_max_v = 0.9
+ base_v = perc_of_max_v * max_base_v
+
+ # 1) the length of the path divided by 0.9 * max_vel
+ # gives us the total time of the trajectory,
+ # so we first compute that
+ # easiest possible way to get approximate path length
+ # (yes it should be from the polynomial approximation but that takes time to write)
+ x_i = path2D_untimed[:,0][:-1] # no last element
+ y_i = path2D_untimed[:,1][:-1] # no last element
+ x_i_plus_1 = path2D_untimed[:,0][1:] # no first element
+ y_i_plus_1 = path2D_untimed[:,1][1:] # no first element
+ x_diff = x_i_plus_1 - x_i
+ x_diff = x_diff.reshape((-1,1))
+ y_diff = y_i_plus_1 - y_i
+ y_diff = y_diff.reshape((-1,1))
+ path_length = np.sum(np.linalg.norm(np.hstack((x_diff, y_diff)), axis=1))
+ total_time = path_length / base_v
+ # 2) we find the correspondence between s and time
+ # TODO: read from where it should be, but seba checked that it's 5 for some reason
+ max_s = 5
+ t_to_s = max_s / total_time
+ # 3) construct the ocp-timed 2D path
+ path2D = []
+ # time is i * args.ocp_dt
+ for i in range(args.n_knots + 1):
+ s = (i * args.ocp_dt) * t_to_s
+ path2D.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
+ path2D = np.array(path2D)
+
+ ####################################################
+ # constructing the SE3 reference from [x,y] path #
+ ####################################################
+ # 1) we need to add points so that there are args.n_knots of them
+ # the -1 is here because we're finite differencing to get theta,
+ # so we need one extra
+ #path2D = list(path2D)
+ ## in case there's too few
+ #while len(path2D) - 1 < args.n_knots:
+ # path2D.append(path2D[-1])
+ ## in case there's too much
+ #while len(path2D) - 1> args.n_knots:
+ # path2D.pop()
+ #path2D = np.array(path2D)
+
+
+ #########################
+ # path2D into pathSE2 #
+ #########################
+ # construct theta
+ # since it's a pairwise operation it can't be done on the last point
+ x_i = path2D[:,0][:-1] # no last element
+ y_i = path2D[:,1][:-1] # no last element
+ x_i_plus_1 = path2D[:,0][1:] # no first element
+ y_i_plus_1 = path2D[:,1][1:] # no first element
+ x_diff = x_i_plus_1 - x_i
+ y_diff = y_i_plus_1 - y_i
+ # elementwise arctan2
+ thetas = np.arctan2(x_diff, y_diff)
+
+ #thetas = thetas.reshape((-1, 1))
+ #path_SE2 = np.hstack((path2D, thetas))
+
+ ######################################
+ # construct SE3 from SE2 reference #
+ ######################################
+ # the plan is parallel to the ground because it's a mobile
+ # manipulation task
+ # TODO: enforce timing, interpolate the path accordingly
+ path = []
+ for i in range(len(path2D) - 1):
+ rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
+ translation = np.array([path2D[i][0], path2D[i][1],
+ args.handlebar_height])
+ path.append(pin.SE3(rotation, translation))
+
+ return path
+
# function to be put into ProcessManager,
# spitting out path points
def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
@@ -501,17 +616,11 @@ def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
break
# Update the scene
+ # r0 is the current position, rg is the goal, rho is a constant
+ # --> why do we need to output this?
r0, rg, rho, obstacles_star = scene_updater.update(p, goal, obstacles)
# compute the path
path_pol, epsilon = path_gen.update(p, r0, rg, rho, obstacles_star)
- # delete comment once new thing works
- ################################################
- # create (N,2) path for list [x0,y0,x1,y1,...]
- # of course it shouldn't be like that to begin with but
- # i have no time for that
- #path2D = np.array(path_gen.target_path)
- #path2D = path2D.reshape((-1, 2))
- ################################################
data_queue.put((path_pol, path_gen.target_path))
if args.debug_prints:
print("put in new plan")
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index 1def7147bf3b5c4ed1bfb7ca66479b3a5142b94a..0c5b24f2f30666787d02956795b99414a79b25c3 100644
GIT binary patch
delta 688
zcmey-$oRaOk?%AwFBby?1B2xqpY-!C8~J8PbFncnFfcPPFnr#zd4seNBctEs!!q9)
zuT0)8`-`PWq=s$sd{!~J6fQ{y28Pv)AiWF>j0`pGDcso_3=BnbHEdamlNYjzn5A%o
zl%(*0$Ql+Xj~B#S&4i?l52US_X#!)9RSjE~^5l&y!ixMgEGc}bDkm`Z1hOzpVC+d#
zW++ja{9ewGQDCy7{CP&f$sgtS%G~0}Pbw}+OwLIy0vTS!&cMKMi=`m3B;yvt<aG+x
zjKY&&DNGT&#hsXv;ty4Mi#;(VB_OdRV{)BhBv(5F1H(!N28QAnlOHO^GRjW2SGt-o
z$?k@T_;jC%J`02wgfEo2BBFOgSZs#YC1I5ZBI48iC;BgNUct4{e}m&i6|)`8JG^%C
zpJ2Rb;dMpC8>CKciY-W)_eAdnObeV>ur2hyCSveGlR-lIBP#>H@Q*JBlZ}<*#a+4G
zr5KJgNP$QxM;lK`=93bPo|2naDj#Fj>SADEC_)ff3=9k*nvAzt@(WV)G+B^jx+c%m
z3}x)v{9e<Wk+E#DwYEBA<>X{-O~!`Fz1p$@RUmV!K|~FRsGYn^Tb{9H@-1xz#@5Nd
zwZm8!GB7Y)+8m}cg_kjB@@vaD#tECftV9_ZS4>W@7Gs<_x!l^1y9K0m7KoTV`GU1S
z<EqIDHleKB7#J9gH&@xbVz!?Fk}C%hq9DQ?L}Y*nQ0NwctSADhUdd1-1`-tk#S({2
mZhlH>PO4qefys*<?8Q_Vbw33$hB7khes*F`Vq_Ess|5gs0Kjhm
delta 462
zcmaFf%=n{`k?%AwFBby?14E^nSNas^jeIktxmXz(7?>Ft7(Ux<-XJZ+$S5%Ru*`SH
zX_I%${$eR+?D;!cQB!R4SveuT35-3$EDRGEdo+|8N)#q%%NsIsPo67(o{?vAuEO5Q
zZi?27f|HvRr%cvSisWiyU|?9uz`#(vd~&~1ETj13w@O##C)?c+5uffe(Px41g7Aeh
z*F^L_$TCPse`ICg7yj`@WwMe=ytoU$n+(G-W=Rk!<7nd{&3r<N(L;Lk43%T7TCEHW
z3`Gbci-Cb5M3eCrOMXFWo+b;DOzY$htx(3+&G)pt85xTwYw4&nmQ4=Q(PXThT%#i^
zP!2Mu0z_1Th^omeb>tcAC!f?&U~HKDN+*nU4g&+jna!@cQ+OHECf~A(W9;5+X)Vgg
zxOlR^jTqyU$!Ruz-1Q)>Q$fVE$@^{m8JAAxw+&_8$iTp$yE)VL6|?<hkX#9f5CswD
zAR+@qfC8roWJM83^-6{!F_5ST0|Ub?4x8Nkl+v73yQ1BbCpp@S$ua7Fa$^i-WYqoa
M#GJ&)C=6B$0CazVlmGw#
diff --git a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
index 781dfc3..e241d05 100644
--- a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
+++ b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
@@ -41,6 +41,8 @@ class MeshcatVisualizer(PMV):
# there will be times when i just want to drop in points
# which will never be changed
self.n_points = 0
+ self.n_path_points = 0
+ self.n_obstacles = 0
if robot is not None:
super().__init__(robot.model, robot.collision_model, robot.visual_model)
elif model is not None:
@@ -75,6 +77,14 @@ class MeshcatVisualizer(PMV):
def addBox(self, name, dims, color):
material = materialFromColor(color)
self.viewer[name].set_object(meshcat.geometry.Box(dims), material)
+
+ def addObstacle(self, pose, dims):
+ color = [0.5, 0.5, 0.5, 0.8]
+ obstacle_name = f"world/obstacle_{self.n_obstacles}"
+ self.addBox(obstacle_name, dims, color)
+ self.applyConfiguration(obstacle_name, pose)
+ self.n_obstacles += 1
+
def addEllipsoid(self, name, dims, color):
material = materialFromColor(color)
@@ -87,9 +97,14 @@ class MeshcatVisualizer(PMV):
self.n_points += 1
def addPath(self, name, path : list[pin.SE3], radius=5e-3, color=[1, 0, 0, 0.8]):
+ # who cares about the name
+ name = "path"
for i, point in enumerate(path):
- self.addSphere(f"world/path_{name}_point_{i}", radius, color)
+ if i < self.n_path_points:
+ self.addSphere(f"world/path_{name}_point_{i}", radius, color)
self.applyConfiguration(f"world/path_{name}_point_{i}", point)
+ self.n_path_points = max(i, self.n_path_points)
+
def applyConfiguration(self, name, placement):
if isinstance(placement, list) or isinstance(placement, tuple):
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index fc25e69..3c039a0 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -191,6 +191,12 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue
viz.display(cmd["q"])
if key == "point":
viz.addPoint(cmd["point"])
+ if key == "obstacle":
+ # stupid and evil but there is no time
+ viz.addObstacle(cmd["obstacle"][0], cmd["obstacle"][1])
+ if key == "path":
+ # stupid and evil but there is no time
+ viz.addPath("", cmd["path"])
except KeyboardInterrupt:
if args.debug_prints:
--
GitLab