def main():
#creates a world and loads all the items on the command line
world = WorldModel()
res = world.readFile("./HRP2_Robot.xml")
if not res:
raise RuntimeError("Unable to load model")
robot = world.robot(0)
# coordinates.setWorldModel(world)
plugin = MyGLPlugin(world)
vis.pushPlugin(plugin)
#add the world to the visualizer
vis.add("world", world)
vis.add("robot", world.robot(0))
vis.show()
# ipdb.set_trace()
Robotstate_Traj, Contact_Force_Traj = Traj_Loader()
h = 0.0068 # 0.0043: vert
# 0.0033: flat
playspeed = 2.5
norm = 500
while vis.shown():
# This is the main plot program
for i in range(0, Robotstate_Traj.shape[1]):
vis.lock()
Robotstate_Traj_i = Robotstate_Traj[:, i]
Robotstate_Traj_Full_i = Dimension_Recovery(Robotstate_Traj_i)
# Now it is the plot of the contact force at the contact extremities
robot.setConfig(Robotstate_Traj_Full_i)
Contact_Force_Traj_i = Contact_Force_Traj[:, i]
left_ft, rght_ft, left_hd, rght_hd, left_ft_end, rght_ft_end, left_hd_end, rght_hd_end = Contact_Force_vec(
robot, Contact_Force_Traj_i, norm)
vis.add("left foot force",
Trajectory([0, 1], [left_ft, left_ft_end]))
vis.add("right foot force",
Trajectory([0, 1], [rght_ft, rght_ft_end]))
vis.add("left hand force",
Trajectory([0, 1], [left_hd, left_hd_end]))
vis.add("right hand force",
Trajectory([0, 1], [rght_hd, rght_hd_end]))
COMPos_start = robot.getCom()
COMPos_end = COMPos_start
COMPos_end[2] = COMPos_end[2] + 100
vis.add("Center of Mass",
Trajectory([0, 1], [COMPos_start, COMPos_end]))
# ipdb.set_trace()
vis.unlock()
time.sleep(h * playspeed)
def callback(robot_controller=robot_controller,
drawing_controller=drawing_controller,
storage=[sim_world, planning_world, sim, controller_vis]):
start_clock = time.time()
dt = 1.0 / robot_controller.controlRate()
#advance the controller
robot_controller.startStep()
if robot_controller.status() == 'ok':
drawing_controller.advance(dt)
vis.addText("Status", drawing_controller.state, (10, 20))
robot_controller.endStep()
#update the visualization
sim_robot.setConfig(
robot_controller.configToKlampt(robot_controller.sensedPosition()))
Tee = sim_robot.link(ee_link).getTransform()
vis.add(
"pen axis",
Trajectory([0, 1], [
se3.apply(Tee, ee_local_pos),
se3.apply(Tee,
vectorops.madd(ee_local_pos, ee_local_axis, lifth))
]),
color=(0, 1, 0, 1))
controller_vis.update()
cur_clock = time.time()
duration = cur_clock - start_clock
time.sleep(max(0, dt - duration))
def test_trajectory_editing():
traj = SE3Trajectory([0, 1], [se3.identity(), se3.identity()])
saved, result = resource.edit("se3 traj",
traj,
'SE3Trajectory',
'Some spatial trajectory',
world=w,
referenceObject=r.link(7))
traj = Trajectory([0, 1], [[0, 0, 0], [0, 0, 1]])
saved, result = resource.edit("point traj",
traj,
'auto',
'Some point trajectory',
world=w,
referenceObject=r.link(7))
traj = RobotTrajectory(r, [0, 1, 2], [q0, qrand, qrand2])
saved, result = resource.edit("robot traj",
traj,
'auto',
'Random robot trajectory',
world=w,
referenceObject=r)
vis.kill()
def RobotCOMPlot(SimRobot, vis):
COMPos_start = SimRobot.getCom()
COMPos_end = COMPos_start[:]
COMPos_end[2] = COMPos_end[2] - 7.50
vis.add("COM", Trajectory([0, 1], [COMPos_start, COMPos_end]))
vis.hideLabel("COM", True)
vis.setColor("COM", 0.0, 204.0 / 255.0, 0.0, 1.0)
vis.setAttribute("COM", 'width', 5.0)
def add_to_vis(self,name,color=(1,0,0,1)):
finger_radius = 0.02
if self.finger_width == None:
w = 0.05
else:
w = self.finger_width*0.5+finger_radius
a = vectorops.madd(self.center,self.axis,w)
b = vectorops.madd(self.center,self.axis,-w)
vis.add(name,Trajectory(milestones=[a,b]),color=color)
if self.approach is not None:
vis.add(name+"_approach",Trajectory(milestones=[self.center,vectorops.madd(self.center,self.approach,0.05)]),color=(1,0.5,0,1))
normallen = 0.05
if self.contact1 is not None:
vis.add(name+"cp1",self.contact1.x,color=(1,1,0,1),size=0.01)
vis.add(name+"cp1_normal",Trajectory(milestones=[self.contact1.x,vectorops.madd(self.contact1.x,self.contact1.n,normallen)]),color=(1,1,0,1))
if self.contact2 is not None:
vis.add(name+"cp2",self.contact2.x,color=(1,1,0,1),size=0.01)
vis.add(name+"cp2_normal",Trajectory(milestones=[self.contact2.x,vectorops.madd(self.contact2.x,self.contact2.n,normallen)]),color=(1,1,0,1))
def COM2IntersectionPlot(i, vis, COM_Pos, Intersection):
# This function is used to plot PIP from COM to intersections
Edge_Index = str(i)
vis.add("PIPEdgefromCOM:" + Edge_Index,
Trajectory([0, 1], [COM_Pos, Intersection]))
vis.hideLabel("PIPEdgefromCOM:" + Edge_Index, True)
vis.setAttribute("PIPEdgefromCOM:" + Edge_Index, 'width', 7.5)
vis.setColor("PIPEdgefromCOM:" + Edge_Index, 65.0 / 255.0, 199.0 / 255.0,
244.0 / 255.0, 1.0)
def PIP_Subplot(i, EdgeA, EdgeB, EdgeCOM, Edgex, Edgey, Edgez, COM_Pos, vis):
scale = 0.25
Edge_Index = str(i)
vis.add("PIPEdge:" + Edge_Index, Trajectory([0, 1], [EdgeA, EdgeB]))
vis.hideLabel("PIPEdge:" + Edge_Index, True)
vis.setAttribute("PIPEdge:" + Edge_Index, 'width', 7.5)
vis.add("PIPEdgeCOM:" + Edge_Index, Trajectory([0, 1], [COM_Pos, EdgeCOM]))
vis.hideLabel("PIPEdgeCOM:" + Edge_Index, True)
vis.setAttribute("PIPEdgeCOM:" + Edge_Index, 'width', 7.5)
vis.setColor("PIPEdgeCOM:" + Edge_Index, 65.0 / 255.0, 199.0 / 255.0,
244.0 / 255.0, 1.0)
# Local Coordinates
Edgex_i = [0.0, 0.0, 0.0]
Edgex_i[0] = EdgeCOM[0] + scale * Edgex[0]
Edgex_i[1] = EdgeCOM[1] + scale * Edgex[1]
Edgex_i[2] = EdgeCOM[2] + scale * Edgex[2]
vis.add("PIPEdgex:" + Edge_Index, Trajectory([0, 1], [EdgeCOM, Edgex_i]))
vis.hideLabel("PIPEdgex:" + Edge_Index, True)
vis.setAttribute("PIPEdgex:" + Edge_Index, 'width', 7.5)
vis.setColor("PIPEdgex:" + Edge_Index, 1.0, 0.0, 0.0, 1.0)
Edgey_i = [0.0, 0.0, 0.0]
Edgey_i[0] = EdgeCOM[0] + scale * Edgey[0]
Edgey_i[1] = EdgeCOM[1] + scale * Edgey[1]
Edgey_i[2] = EdgeCOM[2] + scale * Edgey[2]
vis.add("PIPEdgey:" + Edge_Index, Trajectory([0, 1], [EdgeCOM, Edgey_i]))
vis.hideLabel("PIPEdgey:" + Edge_Index, True)
vis.setAttribute("PIPEdgey:" + Edge_Index, 'width', 7.5)
vis.setColor("PIPEdgey:" + Edge_Index, 155.0 / 255.0, 244.0 / 255.0,
66.0 / 255.0, 1.0)
Edgez_i = [0.0, 0.0, 0.0]
Edgez_i[0] = EdgeCOM[0] + scale * Edgez[0]
Edgez_i[1] = EdgeCOM[1] + scale * Edgez[1]
Edgez_i[2] = EdgeCOM[2] + scale * Edgez[2]
# print Edgez_i
vis.add("PIPEdgez:" + Edge_Index, Trajectory([0, 1], [EdgeCOM, Edgez_i]))
vis.hideLabel("PIPEdgez:" + Edge_Index, True)
vis.setAttribute("PIPEdgez:" + Edge_Index, 'width', 7.5)
vis.setColor("PIPEdgez:" + Edge_Index, 68.0 / 255.0, 65.0 / 255.0,
244.0 / 255.0, 1.0)
def ConvexEdgesPlot(SimRobot, convex_edges_list, vis):
Convex_Edges_Number = len(convex_edges_list) / 2
COMPos = SimRobot.getCom()
for i in range(0, Convex_Edges_Number):
EdgeA = convex_edges_list[2 * i]
EdgeB = convex_edges_list[2 * i + 1]
# Three edges to be added: A->B, A -> COM, B-> COM
Edge_Index = str(i)
vis.add("Edge:" + Edge_Index, Trajectory([0, 1], [EdgeA, EdgeB]))
vis.hideLabel("Edge:" + Edge_Index, True)
vis.setAttribute("Edge:" + Edge_Index, 'width', 5.0)
def update_robot_viz(x, y, z):
vis.clear()
vis.add("robot", display_robot)
for i, o in enumerate(objs):
odisplay = wdisplay.rigidObject(i)
vis.add("obj_{}".format(i), odisplay)
try:
display_robot.setConfig(x)
for i, yi in enumerate(y):
vis.add("pt_{}".format(i),
yi[1],
color=(1, 0, 0, 1),
hide_label=True)
f_normal = z[i * 7 + 6]
f_friction = z[i * 7:i * 7 + 6]
n = np.array(normal(grasping_problem.xyz_eq.env_geom, yi[1]))
f = n * f_normal
n1 = so3.canonical(n)[3:6]
n2 = so3.canonical(n)[6:9]
for j in range(6):
n_tmp = (math.cos(
(math.pi / 3) * j) * np.array(n1) + math.sin(
(math.pi / 3) * j) * np.array(n2))
f += n_tmp * f_friction[j]
# f is the force pointing inward
vis.add(
"n_{}".format(i),
Trajectory([0, 1],
[yi[1], vectorops.madd(yi[1], n, 0.05)]),
color=(1, 1, 0, 1),
hide_label=True)
vis.add(
"f_{}".format(i),
Trajectory(
[0, 1],
[yi[1], vectorops.madd(yi[1], f.tolist(), -1.0)]),
color=(1, 0.5, 0, 1),
hide_label=True)
finally:
pass
time.sleep(0.01)
def get_paths(add_to_vis=True):
"""Returns a list of Trajectory objects describing the desired end effector
motion in 3D space"""
#TODO: design your trajectories here or use get_paths_svg instead
#return get_paths_svg("mypaths.svg",add_to_vis)
trajs = []
trajs.append(Trajectory([0, 1], [[0, 0, 0], [0.2, 0, 0]]))
if add_to_vis:
for i, traj in enumerate(trajs):
vis.add("path %d" % i, traj)
return trajs
def test_debug():
vis.debug(w.robot(0), centerCamera=True)
g = Geometry3D()
g.loadFile("../../data/objects/srimugsmooth.off")
vis.debug(g, centerCamera=True)
pt = [0, 0, 2]
traj = Trajectory([0, 1, 2], [[0, 0, 2], [1, 0, 2], [1, 1, 2]])
vis.debug('qrand', [qrand, qrand2, q0], {'color': [1, 0, 0, 0.5]},
pt,
world=w)
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
pt,
world=w,
centerCamera=r.link(6))
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
world=w,
followCamera=r.link(6))
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
pt=pt,
world=w,
animation=traj)
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
pt=pt,
world=w,
centerCamera='pt')
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
pt=pt,
world=w,
animation=traj,
followCamera='pt')
milestones = []
for i in range(5):
r.randomizeConfig()
milestones.append(r.getConfig())
r.setConfig(q0)
qtraj = RobotTrajectory(r, [0, 1, 2, 3, 4], milestones)
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
world=w,
animation=qtraj)
#this doesn't work -- qrand is not being tracked
vis.debug('qrand',
qrand, {'color': [1, 0, 0, 0.5]},
world=w,
animation=qtraj,
followCamera=r.link(6))
def main(*arg):
Robot_Option = "../user/"
world = WorldModel() # WorldModel is a pre-defined class
EnviName = "/home/motion/Desktop/Whole-Body-Planning-for-Push-Recovery-Data/flat_1Contact/"
# The next step is to load in robot's XML file
XML_path = EnviName + "Envi.xml"
result = world.readFile(
XML_path
) # Here result is a boolean variable indicating the result of this loading operation
if not result:
raise RuntimeError("Unable to load model " + XML_path)
# In this case, what we have is a config
CtrlStateTraj = Trajectory(world.robot(0))
CaseNo = 1
TestNo = 5
SpecificPath = EnviName + "/" + str(CaseNo) + "/" + str(TestNo)
CtrlStateTraj.load(SpecificPath + "/CtrlStateTraj.path")
import ipdb
ipdb.set_trace()
Config_Init = CtrlStateTraj.milestones[-1]
Robot_Config_Plot(world, len(Config_Init), Config_Init)
def toTrajectory(self):
if self.type == 'Trajectory':
return
elif self.type == 'Configs':
self.type = 'Trajectory'
else:
times = []
milestones = []
for i, s in enumerate(self.data.sections):
if i == 0:
if s.times is not None:
times += s.times
milestones += s.configs
else:
if s.times is not None:
times += s.times[1:]
milestones += s.configs[1:]
if len(times) == 0:
times = list(range(len(milestones)))
else:
assert len(times) == len(milestones)
self.data = Trajectory(times, milestones)
self.type = 'Trajectory'
def svg_path_to_trajectory(p):
"""Produces either a Trajectory or HermiteTrajectory according to an SVG
Path. The path must be continuous.
"""
from svgpathtools import Path, Line, QuadraticBezier, CubicBezier, Arc
if not p.iscontinuous():
raise ValueError("Can't do discontinuous paths")
pwl = not any(isinstance(seg, (CubicBezier, QuadraticBezier)) for seg in p)
if pwl:
milestones = []
for seg in p:
a, b = seg.start, seg.end
if not milestones:
milestones.append([a.real, a.imag])
milestones.append([b.real, b.imag])
return Trajectory(list(range(len(milestones))), milestones)
times = []
milestones = []
for i, seg in enumerate(p):
if isinstance(seg, CubicBezier):
a, c1, c2, b = seg.start, seg.control1, seg.control2, seg.end
vstart = (c1.real - a.real) * 3, (c1.imag - a.imag) * 3
vend = (b.real - c2.real) * 3, (b.imag - c2.imag) * 3
if not milestones:
milestones.append([a.real, a.imag, vstart[0], vstart[1]])
times.append(i)
elif vectorops.distance(milestones[-1][2:], vstart) > 1e-4:
milestones.append([a.real, a.imag, 0, 0])
times.append(i)
milestones.append([a.real, a.imag, vstart[0], vstart[1]])
times.append(i)
milestones.append([b.real, b.imag, vend[0], vend[1]])
times.append(i + 1)
elif isinstance(seg, Line):
a, b = seg.start, seg.end
if not milestones:
milestones.append([a.real, a.imag, 0, 0])
times.append(i)
elif vectorops.norm(milestones[-1][2:]) > 1e-4:
milestones.append([a.real, a.imag, 0, 0])
times.append(i)
milestones.append([b.real, b.imag, 0, 0])
times.append(i + 1)
else:
raise NotImplementedError(
"Can't handle pieces of type {} yet".format(
seg.__class.__name__))
return HermiteTrajectory(times, milestones)
def TransitionDataPlot(vis, StepNo, LimbNo, ReachableContacts_data):
RowNo, ColumnNo = ReachableContacts_data.shape
RowStart = 0
RowEnd = RowNo
Traj = []
for i in range(RowStart, RowEnd):
point_i = [0.0, 0.0, 0.0]
ReachableContact_i = ReachableContacts_data[i]
point_i[0] = ReachableContact_i[0]
point_i[1] = ReachableContact_i[1]
point_i[2] = ReachableContact_i[2]
Traj.append(point_i)
TransName = "Stage" + str(StepNo) + "LinkNo" + str(LimbNo) + "Path"
vis.add(TransName, Trajectory([0, 1], Traj))
vis.hideLabel(TransName, True)
vis.setColor(TransName, 255.0 / 255.0, 255.0 / 255.0, 51.0 / 255.0, 1.0)
def draw_path(self, pos1, pos2, colour=None):
"""Draw a path (line) in the klampt visualiser
:param pos1: the position of the start of line
:param pos2: the position of the end of line
:param colour: the colour of the line
"""
from klampt import vis
from klampt.model.trajectory import Trajectory
unique_label = repr((pos1, pos2))
label = repr((pos1, pos2))
if label not in self.drawn_label:
vis.add(label, Trajectory(milestones=[pos1, pos2]), keepAppearance=True)
if colour is not None:
vis.setAttribute(label, "color", colour)
vis.hideLabel(label)
def load(self, fn):
if fn.endswith('xml'):
self.type = 'MultiPath'
self.data = MultiPath()
self.data.load(fn)
elif fn.endswith('path') or fn.endswith('traj'):
self.type = 'Trajectory'
self.data = Trajectory()
self.data.load(fn)
elif fn.endswith('configs') or fn.endswith('milestones'):
self.type = 'Configs'
self.data = Trajectory()
self.data.configs = loader.load('Configs', fn)
self.data.times = list(range(len(self.data.configs)))
else:
print("Unknown extension on file", fn, ", loading as Trajectory")
self.type = 'Trajectory'
self.data = Trajectory()
self.data.load(fn)
def WeightedContactDataPlot(vis, OptimalContact_data,
OptimalContactWeights_data):
scale = 1.0
for i in range(OptimalContact_data.size / 3):
point_start = [0.0, 0.0, 0.0]
ReachableContact_i = OptimalContact_data[i]
point_start[0] = ReachableContact_i[0]
point_start[1] = ReachableContact_i[1]
point_start[2] = ReachableContact_i[2]
point_end = [0.0, 0.0, 0.0]
ReachableContactWeight_i = OptimalContactWeights_data[i]
point_end[0] = point_start[0] + scale * ReachableContactWeight_i[0]
point_end[1] = point_start[1] + scale * ReachableContactWeight_i[1]
point_end[2] = point_start[2] + scale * ReachableContactWeight_i[2]
print i
vis.add("PointWeights_" + str(i),
Trajectory([0, 1], [point_start, point_end]))
vis.hideLabel("PointWeights_" + str(i), True)
vis.setColor("PointWeights_" + str(i), 0.0, 204.0 / 255.0, 0.0, 1.0)
vis.setAttribute("PointWeights_" + str(i), 'width', 5.0)
def ImpulsePlot(vis, SimRobot, SimulationTime, ImpulseObj):
StartTime = ImpulseObj[0]
EndTime = ImpulseObj[1]
ImpulForce = ImpulseObj[2]
DurationTime = EndTime - StartTime
OriPoint = SimRobot.link(19).getWorldPosition([0.0, 0.0, 0.0])
ImpulseScale = 0.001 * (SimulationTime - StartTime) / DurationTime
EndPoint = OriPoint[:]
EndPoint[0] += ImpulForce[0] * ImpulseScale
EndPoint[1] += ImpulForce[1] * ImpulseScale
EndPoint[2] += ImpulForce[2] * ImpulseScale
if SimulationTime < StartTime:
pass
elif SimulationTime > EndTime:
vis.hide("Impulse", True)
else:
vis.add("Impulse", Trajectory([0, 1], [OriPoint, EndPoint]))
vis.hideLabel("Impulse", True)
vis.setColor("Impulse", 235.0 / 255.0, 52.0 / 255.0, 52.0 / 255.0, 1.0)
vis.setAttribute("Impulse", 'width', 5.0)
def get_paths(add_to_vis=True):
"""Returns a list of Trajectory objects describing the desired end effector
motion in 3D space"""
#TODO: design your trajectories here or use get_paths_svg instead
#return get_paths_svg("mypaths.svg",add_to_vis)
trajs = []
trajs.append(Trajectory([0,1,2,2.5,3,4,4.5,5],[[0,0,0],[0,0.2,0],[0.07,0.2,0],[0.1,0.15,0],[0.07,0.1,0],[0,0.1,0],[0,0.1,0.1],[0,0.1,0.1]]))
trajs.append(Trajectory([0,1,2,2.5,3,3.5,4], [[0,0.1,0.1],[0.3,0,0],[0.3,0.1,0],[0.35,0.1,0],[0.35,0.08,0],[0.35,0.08,0.1],[0.35,0.08,0.1]]))
trajs.append(Trajectory([0,1,1.5,2,2.5,3,3.5,4], [[0.35,0.08,0.1],[0.45,0.03,0],[0.4,0,0],[0.4,0.1,0],[0.45,0.1,0],[0.45,0,0],[0.45,0,0.1],[0.45,0,0.1]]))
trajs.append(Trajectory([0,1,1.5,2,2.5,3,3.5], [[0.45,0,0.1],[0.5,0,0],[0.5,0.1,0],[0.55,0.1,0],[0.55,0,0],[0.55,0,0.1],[0.55,0,0.1]]))
trajs.append(Trajectory([0,1,1.5,2,2.5,3,3.5,4], [[0.55,0,0.1],[0.65,0.03,0],[0.6,0,0],[0.6,0.1,0],[0.65,0.1,0],[0.65,0,0],[0.65,0,0.1],[0.65,0,0.1]]))
trajs.append(Trajectory([0,1,1.5,2,2.5,3,3.5], [[0.65,0,0.1],[0.7,0.1,0],[0.75,0,0],[0.8,0.1,0],[0.8,0.1,0.1],[0.8,0.1,0.1]]))
if add_to_vis:
for i,traj in enumerate(trajs):
vis.add("path %d"%i,traj)
return trajs
def WeightedContactDataPlot(vis, StepNo, LimbNo, OptimalContact_data,
OptimalContactWeights_data):
scale = 1.0
for i in range(OptimalContact_data.size / 3):
point_start = [0.0, 0.0, 0.0]
ReachableContact_i = OptimalContact_data[i]
point_start[0] = ReachableContact_i[0]
point_start[1] = ReachableContact_i[1]
point_start[2] = ReachableContact_i[2]
point_end = [0.0, 0.0, 0.0]
ReachableContactWeight_i = OptimalContactWeights_data[i]
point_end[0] = point_start[0] + scale * ReachableContactWeight_i[0]
point_end[1] = point_start[1] + scale * ReachableContactWeight_i[1]
point_end[2] = point_start[2] + scale * ReachableContactWeight_i[2]
ContactName = "Stage" + str(StepNo) + "LinkNo" + str(
LimbNo) + "Point:" + str(i)
print i
vis.add(ContactName, Trajectory([0, 1], [point_start, point_end]))
vis.hideLabel(ContactName, True)
vis.setColor(ContactName, 0.0, 204.0 / 255.0, 0.0, 1.0)
vis.setAttribute(ContactName, 'width', 5.0)
def from_JointTrajectory(ros_traj, robot=None, joint_link_indices=None):
"""Returns a Klamp't Trajectory or RobotTrajectory for a JointTrajectory
message.
Args:
ros_traj (sensor_msgs.msg.JointTrajectory): the JointTrajectory object
robot (RobotModel): the robot, optional
joint_link_indices (dict, optional): if given, maps ROS joint names to
Klampt link indices. Default uses the Klamp't link names.
"""
from klampt.model.trajectory import Trajectory, RobotTrajectory
if robot is None:
res = Trajectory()
for i in range(len(ros_traj.points)):
res.times.append(ros_traj.points[i].time_from_start.toSec())
res.milestones.append([v for v in ros_traj.points[i].positions])
return res
else:
res = RobotTrajectory(robot)
q0 = robot.getConfig()
if joint_link_indices is None:
joint_link_indices = dict()
for i in range(robot.numLinks()):
joint_link_indices[robot.link(i).getName()] = i
indices = []
if len(ros_traj.joint_names) > 0:
for n in ros_traj.joint_names:
if n not in joint_link_indices:
raise ValueError("ROS joint name %s is invalid" % (n, ))
indices = joint_link_indices[n]
for i in range(len(ros_traj.points)):
res.times.append(ros_traj.points[i].time_from_start.toSec())
for j, k in enumerate(indices):
q0[k] = ros_traj.points[i].positions[j]
res.milestones.append([v for v in q0])
return res
def planTriggered():
global world, robot, obj, target_gripper, solved_trajectory, trajectory_is_transfer, Tobject_grasp, obj, next_item_to_pick, qstart
if PROBLEM == '3a':
robot.setConfig(qstart)
res = place.transfer_plan(world, robot, qgoal, obj, Tobject_grasp)
if res is None:
print("Unable to plan transfer")
else:
traj = RobotTrajectory(robot, milestones=res)
vis.add("traj", traj, endEffectors=[gripper_link.index])
solved_trajectory = traj
robot.setConfig(qstart)
elif PROBLEM == '3b':
res = place.plan_place(world, robot, obj, Tobject_grasp,
target_gripper, goal_bounds)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
traj = transfer
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
traj = traj.concat(retract, relative=True, jumpPolicy='jump')
vis.add("traj", traj, endEffectors=[gripper_link.index])
solved_trajectory = traj
robot.setConfig(qstart)
else:
robot.setConfig(qstart)
obj = world.rigidObject(next_item_to_pick)
Tobj0 = obj.getTransform()
print("STARTING TO PICK OBJECT", obj.getName())
orig_grasps = db.object_to_grasps[obj.getName()]
grasps = [
grasp.get_transformed(obj.getTransform()).transfer(
source_gripper, target_gripper) for grasp in orig_grasps
]
res = pick.plan_pick_multistep(world, robot, obj, target_gripper,
grasps)
if res is None:
print("Unable to plan pick")
else:
transit, approach, lift = res
qgrasp = approach.milestones[-1]
#get the grasp transform
robot.setConfig(qgrasp)
Tobj = obj.getTransform()
Tobject_grasp = se3.mul(se3.inv(gripper_link.getTransform()),
Tobj)
robot.setConfig(lift.milestones[-1])
res = place.plan_place(world, robot, obj, Tobject_grasp,
target_gripper, goal_bounds)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
trajectory_is_transfer = Trajectory()
trajectory_is_transfer.times.append(0)
trajectory_is_transfer.milestones.append([0])
traj = transit
traj = traj.concat(approach,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
trajectory_is_transfer.milestones.append([1])
traj = traj.concat(lift, relative=True, jumpPolicy='jump')
traj = traj.concat(transfer,
relative=True,
jumpPolicy='jump')
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([1])
trajectory_is_transfer.milestones.append([0])
traj = traj.concat(retract,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
solved_trajectory = traj
obj.setTransform(*Tobj0)
vis.add("traj", traj, endEffectors=[gripper_link.index])
robot.setConfig(qstart)
def run_simulation(world):
value = resource.get('start.config',default=world.robot(0).getConfig(),world=world)
world.robot(0).setConfig(value)
sim_world = world
sim_robot = world.robot(0)
vis.add("world",world)
planning_world = world.copy()
planning_robot = planning_world.robot(0)
sim = Simulator(sim_world)
# robot_controller = RobotInterfaceCompleter(KinematicSimControlInterface(sim_robot))
#TODO: Uncomment this when you are ready for testing in the physics simulation
robot_controller = RobotInterfaceCompleter(SimPositionControlInterface(sim.controller(0),sim))
if not robot_controller.initialize():
raise RuntimeError("Can't connect to robot controller")
ee_link = 'EndEffector_Link'
ee_local_pos = (0.15,0,0)
ee_local_axis = (1,0,0)
lifth = 0.05
drawing_controller = CircleController(robot_controller,planning_robot,ee_link,
ee_local_pos,ee_local_axis,
radius=0.05,period=5.0)
controller_vis = RobotInterfacetoVis(robot_controller)
trace = Trajectory()
#note: this "storage" argument is only necessary for jupyter to keep these around and not destroy them once main() returns
def callback(robot_controller=robot_controller,drawing_controller=drawing_controller,trace=trace,
storage=[sim_world,planning_world,sim,controller_vis]):
start_clock = time.time()
dt = 1.0/robot_controller.controlRate()
#advance the controller
robot_controller.startStep()
drawing_controller.advance(dt)
robot_controller.endStep()
#update the visualization
sim_robot.setConfig(robot_controller.configToKlampt(robot_controller.sensedPosition()))
Tee=sim_robot.link(ee_link).getTransform()
wp = se3.apply(Tee,ee_local_pos)
if len(trace.milestones) == 0 or vectorops.distance(wp,trace.milestones[-1]) > 0.01:
trace.milestones.append(wp)
trace.times.append(0)
if len(trace.milestones)==2:
vis.add("trace",trace,color=(0,1,1,1),pointSize=0)
if len(trace.milestones) > 200:
trace.milestones = trace.milestones[-100:]
trace.times = trace.times[-100:]
if len(trace.milestones)>2:
if _backend=='IPython':
vis.remove("trace")
vis.add("trace",trace,color=(0,1,1,1),pointSize=0)
else:
vis.dirty("trace")
controller_vis.update()
cur_clock = time.time()
duration = cur_clock - start_clock
time.sleep(max(0,dt-duration))
vis.loop(callback=callback)
def planTriggered():
global world,robot, robot2, swab,target_gripper,grasp_swab, grasp_plate, solved_trajectory, \
trajectory_is_transfer, next_robot_to_move
########################## Plan for robot0 & swab ############################
if next_robot_to_move == 0:
# plan pick
Tobj0 = swab.getTransform()
# center of tube: (0.5 0.025 0.72)
offset = 0.005
goal_bounds = [(0.615 - offset, 0.025 - offset / 4, 0.85),
(0.615 + offset, 0.025 + offset / 4, 1.0)]
qstart = robot.getConfig()
res = pick.plan_pick_one(world,
robot,
swab,
target_gripper,
grasp_swab,
robot0=True)
if res is None:
print("Unable to plan pick")
else:
print("plan pick success")
transit, approach, lift = res
# plan place
qgrasp = lift.milestones[-1]
robot.setConfig(qgrasp)
Tobj = swab.getTransform() # swab in world frame
# swab in gripper frame
Tobject_grasp = se3.mul(se3.inv(gripper_link.getTransform()),
Tobj)
place.robot0 = True
res = place.plan_place(world, robot, swab, Tobject_grasp,
target_gripper, goal_bounds, True)
robot.setConfig(qstart)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
trajectory_is_transfer = Trajectory()
trajectory_is_transfer.times.append(0)
trajectory_is_transfer.milestones.append([0])
traj = transit
traj = traj.concat(approach,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
trajectory_is_transfer.milestones.append([1])
traj = traj.concat(lift, relative=True, jumpPolicy='jump')
traj = traj.concat(transfer,
relative=True,
jumpPolicy='jump')
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([1])
trajectory_is_transfer.milestones.append([0])
solved_trajectory = traj
swab.setTransform(*Tobj0)
vis.add("traj", traj, endEffectors=[gripper_link.index])
############################# Plan for Plate & robot 2 starts here! #################################
elif next_robot_to_move == 1:
Tobj0 = plate.getTransform()
goal_bounds = [(0.7, 0.62, 0.71), (0.7, 0.62, 0.71)]
res = pick.plan_pick_one(world,
robot2,
plate,
target_gripper,
grasp_plate,
robot0=False)
if res is None:
print("Unable to plan pick")
else:
print("plan pick success")
transit, approach, lift = res
qgrasp = lift.milestones[-1]
robot2.setConfig(qgrasp)
Tobj = plate.getTransform() # plate in world frame
# plate in gripper frame
Tobject_grasp = se3.mul(se3.inv(robot2.link(9).getTransform()),
Tobj)
place.robot0 = False
res = place.plan_place(world, robot2, plate, Tobject_grasp,
target_gripper, goal_bounds, False)
if res is None:
print("Unable to plan place")
else:
(transfer, lower, retract) = res
trajectory_is_transfer = Trajectory()
trajectory_is_transfer.times.append(0)
trajectory_is_transfer.milestones.append([0])
traj = transit
traj = traj.concat(approach,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
trajectory_is_transfer.milestones.append([1])
traj = traj.concat(lift, relative=True, jumpPolicy='jump')
traj = traj.concat(transfer,
relative=True,
jumpPolicy='jump')
traj = traj.concat(lower, relative=True, jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([1])
trajectory_is_transfer.milestones.append([0])
traj = traj.concat(retract,
relative=True,
jumpPolicy='jump')
trajectory_is_transfer.times.append(traj.endTime())
trajectory_is_transfer.milestones.append([0])
solved_trajectory = traj
plate.setTransform(*Tobj0)
vis.add("traj", traj, endEffectors=[robot2.link(9).index])
def main():
ExpNo = 0
StateType = "P"
VisMode = "Traj"
if (len(sys.argv[1:]) == 1):
ExpNo = int(sys.argv[1:][0])
elif (len(sys.argv[1:]) == 2):
ExpNo = int(sys.argv[1:][0])
StateType = sys.argv[1:][1]
else:
ExpNo = int(sys.argv[1:][0])
StateType = sys.argv[1:][1]
VisChoice = str(sys.argv[1:][2])
if VisChoice == "T" or VisChoice == "Traj":
VisMode = "Traj"
if VisChoice == "Poly" or VisChoice == "Polytope":
VisMode = "Poly"
if VisChoice == "PIP":
VisMode = "PIP"
world = WorldModel() # WorldModel is a pre-defined class
curDir = os.getcwd()
CurCasePath = curDir[0:-4] + "-Data/result/" + CurCase
XML_path = CurCasePath + "/Environment.xml"
result = world.readFile(
XML_path
) # Here result is a boolean variable indicating the result of this loading operation
if not result:
raise RuntimeError("Unable to load model " + XML_path)
ContactLinkDictionary = ContactLinkReader(curDir +
"/../user/ContactLink.txt")
PlanStateTraj = Trajectory(world.robot(0))
CtrlStateTraj = Trajectory(world.robot(0))
FailureStateTraj = Trajectory(world.robot(0))
# SpecificPath = CurCasePath + "/" + str(ExpNo) + '/Backup'
SpecificPath = CurCasePath + "/" + str(ExpNo)
PlanStateTraj.load(SpecificPath + "/PlanStateTraj.path")
CtrlStateTraj.load(SpecificPath + "/CtrlStateTraj.path")
FailureStateTraj.load(SpecificPath + "/FailureStateTraj.path")
ExpTraj = [FailureStateTraj, CtrlStateTraj, PlanStateTraj]
PIPInfoList = PIPTrajReader(SpecificPath)
StartTime, EndTime, ImpulForce = ImpulseInfoReader(SpecificPath)
ImpulseObj = [StartTime, EndTime, ImpulForce]
PlanningObj = []
try:
PlanningTimeList, PlanningResList = PlanningInfoReader(SpecificPath)
PlanningObj = [PlanningTimeList, PlanningResList]
except IOError:
print "Initial Push Does Not Cause Fall!"
ExperimentVisualizer(world, ContactLinkDictionary, ExpTraj, PIPInfoList,
ImpulseObj, PlanningObj, SpecificPath,
[StateType, VisMode])
a = atypes['GeometricPrimitive']
b = btypes['GeometricPrimitive']
vis.add("A", a)
vis.add("B", b)
vis.setColor("A", 1, 0, 0, 0.5)
vis.setColor("B", 0, 1, 0, 0.5)
Ta = se3.identity()
Tb = [so3.identity(), [1, 0, 0]]
vis.add("Ta", Ta)
vis.add("Tb", Tb)
vis.edit("Ta")
vis.edit("Tb")
ray = ([-3, 0, 0], [1, 0, 0])
vis.add("ray",
Trajectory([0, 1], [ray[0], vectorops.madd(ray[0], ray[1], 20)]),
color=[1, 0.5, 0, 1])
vis.add("hitpt", [0, 0, 0], color=[1, 0, 1, 1])
def convert(geom, type, label):
global a, b, atypes, btypes, Ta, Tb
if label == 'A':
vis.add(label, atypes[type])
vis.setColor(label, 1, 0, 0, 0.5)
a = atypes[type]
a.setCurrentTransform(*Ta)
else:
vis.add(label, btypes[type])
vis.setColor(label, 0, 1, 0, 0.5)
b = btypes[type]
class GeneralizedPath:
def __init__(self):
self.type = None
self.data = None
def load(self, fn):
if fn.endswith('xml'):
self.type = 'MultiPath'
self.data = MultiPath()
self.data.load(fn)
elif fn.endswith('path') or fn.endswith('traj'):
self.type = 'Trajectory'
self.data = Trajectory()
self.data.load(fn)
elif fn.endswith('configs') or fn.endswith('milestones'):
self.type = 'Configs'
self.data = Trajectory()
self.data.configs = loader.load('Configs', fn)
self.data.times = list(range(len(self.data.configs)))
else:
print("Unknown extension on file", fn, ", loading as Trajectory")
self.type = 'Trajectory'
self.data = Trajectory()
self.data.load(fn)
def save(self, fn):
if fn.endswith('xml') and self.type != 'MultiPath':
self.toMultipath()
self.data.save(fn)
def hasTiming(self):
if self.type == 'Configs':
return False
if self.type == 'Trajectory':
return True
return self.data.hasTiming()
def duration(self):
if self.type == 'MultiPath':
return self.data.endTime()
else:
return self.data.duration()
def start(self):
if self.type == 'MultiPath':
return self.data.sections[0].configs[0]
else:
return self.data.milestones[0]
def end(self):
if self.type == 'MultiPath':
return self.data.sections[-1].configs[-1]
else:
return self.data.milestones[-1]
def timescale(self, scale, ofs):
rev = (scale < 0)
if rev:
scale = -scale
if self.type == 'MultiPath':
if not self.data.hasTiming():
print("MultiPath timescale(): Path does not have timing")
return
et = self.data.endTime()
for s in self.data.sections:
s.times = [ofs + scale * (et - t) for t in s.times]
s.times.reverse()
s.configs.reverse()
if s.velocities:
s.velocities.reverse()
self.data.sections.reverse()
else:
et = self.data.times[-1]
self.data.times = [
ofs + scale * (et - t) for t in self.data.times
]
self.data.times.reverse()
self.data.milestones.reverse()
if self.type == 'Configs':
if scale != 1 or ofs != 0:
self.type = 'Trajectory'
else:
if self.type == 'MultiPath':
if not self.hasTiming():
print("MultiPath timescale(): Path does not have timing")
return
for s in self.data.sections:
s.times = [ofs + scale * t for t in s.times]
else:
self.data.times = [ofs + scale * t for t in self.data.times]
if self.type == 'Configs':
if scale != 1 or ofs != 0:
self.type = 'Trajectory'
def toMultipath(self):
if self.type == 'MultiPath':
return
else:
traj = self.data
mpath = MultiPath()
mpath.sections.append(MultiPath.Section())
mpath.sections[0].times = traj.times
mpath.sections[0].configs = traj.milestones
self.data = mpath
self.type = 'MultiPath'
def toTrajectory(self):
if self.type == 'Trajectory':
return
elif self.type == 'Configs':
self.type = 'Trajectory'
else:
times = []
milestones = []
for i, s in enumerate(self.data.sections):
if i == 0:
if s.times is not None:
times += s.times
milestones += s.configs
else:
if s.times is not None:
times += s.times[1:]
milestones += s.configs[1:]
if len(times) == 0:
times = list(range(len(milestones)))
else:
assert len(times) == len(milestones)
self.data = Trajectory(times, milestones)
self.type = 'Trajectory'
def toConfigs(self):
if self.type == 'Trajectory':
self.type = 'Configs'
elif self.type == 'Configs':
return
else:
self.toTrajectory()
self.data.times = list(range(len(self.data.milestones)))
self.type = 'Configs'
def concat(self, other):
"""Warning: destructive"""
if self.type == 'MultiPath':
other.toMultipath()
elif other.type == 'MultiPath':
self.toMultipath()
if self.type == 'MultiPath':
self.data.concat(other.data)
else:
self.data.concat(other.data, relative=True)
def discretize(self, dt):
if self.type == 'MultiPath':
print("Warning, MultiPath discretize is not supported yet")
else:
self.data = self.data.discretize(dt)
def __str__(self):
if self.type == 'MultiPath':
ndofs = 0
if len(self.data.sections) > 0:
ndofs = len(self.data.sections[0].configs[0])
res = "MultiPath with %d DOFs, %d sections\n" % (
ndofs, len(self.data.sections))
sstrs = []
for i, s in enumerate(self.data.sections):
sres = " Section %d: %d milestones" % (i, len(s.configs))
if s.times != None:
sres += " / times"
if s.velocities != None:
sres += " / velocities"
sres += ", " + str(len(self.data.getStance(i))) + " holds"
sstrs.append(sres)
return res + '\n'.join(sstrs)
else:
ndofs = 0
if len(self.data.milestones) > 0:
ndofs = len(self.data.milestones[0])
return '%s with %d DOFs, %d milestones' % (
self.type, ndofs, len(self.data.milestones))
def add_to_vis(self,robot=None,animate=True,base_xform=None):
"""Adds the gripper to the klampt.vis scene."""
from klampt import vis
from klampt import WorldModel,Geometry3D,GeometricPrimitive
from klampt.model.trajectory import Trajectory
prefix = "gripper_"+self.name
if robot is None and self.klampt_model is not None:
w = WorldModel()
if w.readFile(self.klampt_model):
robot = w.robot(0)
vis.add(prefix+"_gripper",w)
robotPath = (prefix+"_gripper",robot.getName())
elif robot is not None:
vis.add(prefix+"_gripper",robot)
robotPath = prefix+"_gripper"
if robot is not None:
assert self.base_link >= 0 and self.base_link < robot.numLinks()
robot.link(self.base_link).appearance().setColor(1,0.75,0.5)
if base_xform is None:
base_xform = robot.link(self.base_link).getTransform()
else:
if robot.link(self.base_link).getParent() >= 0:
print("Warning, setting base link transform for an attached gripper base")
#robot.link(self.base_link).setParent(-1)
robot.link(self.base_link).setParentTransform(*base_xform)
robot.setConfig(robot.getConfig())
for l in self.finger_links:
assert l >= 0 and l < robot.numLinks()
robot.link(l).appearance().setColor(1,1,0.5)
if robot is not None and animate:
q0 = robot.getConfig()
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(1)
else:
robot.driver(i).setValue(1)
traj = Trajectory([0],[robot.getConfig()])
for i in self.finger_drivers:
if isinstance(i,(list,tuple)):
for j in i:
robot.driver(j).setValue(0)
traj.times.append(traj.endTime()+0.5)
traj.milestones.append(robot.getConfig())
else:
robot.driver(i).setValue(0)
traj.times.append(traj.endTime()+1)
traj.milestones.append(robot.getConfig())
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
traj.times.append(traj.endTime()+1)
traj.milestones.append(traj.milestones[0])
assert len(traj.times) == len(traj.milestones)
traj.checkValid()
vis.animate(robotPath,traj)
robot.setConfig(q0)
if self.center is not None:
vis.add(prefix+"_center",se3.apply(base_xform,self.center))
center_point = (0,0,0) if self.center is None else self.center
outer_point = (0,0,0)
if self.primary_axis is not None:
length = 0.1 if self.finger_length is None else self.finger_length
outer_point = vectorops.madd(self.center,self.primary_axis,length)
line = Trajectory([0,1],[self.center,outer_point])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_primary",line,color=(1,0,0,1))
if self.secondary_axis is not None:
width = 0.1 if self.maximum_span is None else self.maximum_span
line = Trajectory([0,1],[vectorops.madd(outer_point,self.secondary_axis,-0.5*width),vectorops.madd(outer_point,self.secondary_axis,0.5*width)])
line.milestones = [se3.apply(base_xform,m) for m in line.milestones]
vis.add(prefix+"_secondary",line,color=(0,1,0,1))
elif self.maximum_span is not None:
#assume vacuum gripper?
p = GeometricPrimitive()
p.setSphere(outer_point,self.maximum_span)
g = Geometry3D()
g.set(p)
vis.add(prefix+"_opening",g,color=(0,1,0,0.25))
def from_numpy(obj, type='auto', template=None):
"""Converts a numpy array or multiple numpy arrays to a Klamp't object.
Supports:
* lists and tuples
* RigidTransform: accepts a 4x4 homogeneous coordinate transform
* Matrix3, Rotation: accepts a 3x3 matrix.
* Configs
* Trajectory: accepts a pair (times,milestones)
* TriangleMesh: accepts a pair (verts,indices)
* PointCloud: accepts a n x (3+k) array, where k is the # of properties
* VolumeGrid: accepts a triple (bmin,bmax,array)
* Geometry3D: accepts a pair (T,geomdata)
"""
global supportedTypes
if type == 'auto' and template is not None:
otype = types.objectToTypes(template)
if isinstance(otype, (list, tuple)):
for t in otype:
if t in supportedTypes:
type = t
break
if type == 'auto':
raise ValueError('obj is not a supported type: ' +
', '.join(otype))
else:
type = otype
if type == 'auto':
if isinstance(obj, (tuple, list)):
if all(isinstance(v, np.ndarray) for v in obj):
if len(obj) == 2:
if len(obj[0].shape) == 1 and len(obj[1].shape) == 2:
type = 'Trajectory'
elif len(obj[0].shape) == 2 and len(
obj[1].shape
) == 2 and obj[0].shape[1] == 3 and obj[1].shape[1] == 3:
type = 'TriangleMesh'
if len(obj) == 3:
if obj[0].shape == (3, ) and obj[1].shape == (3, ):
type = 'VolumeGrid'
if type == 'auto':
raise ValueError(
"Can't auto-detect type of list of shapes" +
', '.join(str(v.shape) for v in obj))
else:
if isinstance(obj[0], np.ndarray) and obj[0].shape == (4, 4):
type = 'Geometry3D'
else:
raise ValueError(
"Can't auto-detect type of irregular list")
else:
assert isinstance(
obj, np.ndarray
), "Can only convert lists, tuples, and arrays from numpy"
if obj.shape == (3, 3):
type = 'Matrix3'
elif obj.shape == (4, 4):
type = 'RigidTransform'
elif len(obj.shape) == 1:
type = 'Config'
else:
raise ValueError("Can't auto-detect type of matrix of shape " +
str(obj.shape))
if type not in supportedTypes:
raise ValueError(type + ' is not a supported type')
if type == 'RigidTransform':
return se3.from_homogeneous(obj)
elif type == 'Rotation' or type == 'Matrix3':
return so3.from_matrix(obj)
elif type == 'Trajectory':
assert len(obj) == 2, "Trajectory format is (times,milestones)"
times = obj[0].tolist()
milestones = obj[1].tolist()
if template is not None:
return template.constructor()(times, milestones)
from klampt.model.trajectory import Trajectory
return Trajectory(times, milestones)
elif type == 'TriangleMesh':
from klampt import TriangleMesh
res = TriangleMesh()
vflat = obj[0].flatten()
res.vertices.resize(len(vflat))
for i, v in enumerate(vflat):
res.vertices[i] = float(v)
iflat = obj[1].flatten()
res.indices.resize(len(iflat))
for i, v in enumerate(iflat):
res.indices[i] = int(v)
return res
elif type == 'PointCloud':
from klampt import PointCloud
assert len(obj.shape) == 2, "PointCloud array must be a 2D array"
assert obj.shape[1] >= 3, "PointCloud array must have at least 3 values"
points = obj[:, :3]
properties = obj[:, 3:]
res = PointCloud()
res.setPoints(points.shape[0], points.flatten())
if template is not None:
if len(template.propertyNames) != properties.shape[1]:
raise ValueError(
"Template object doesn't have the same properties as the numpy object"
)
for i in range(len(template.propertyNames)):
res.propertyNames[i] = template.propertyNames[i]
else:
for i in range(properties.shape[1]):
res.propertyNames.append('property %d' % (i + 1))
if len(res.propertyNames) > 0:
res.properties.resize(len(res.propertyNames) * points.shape[0])
if obj.shape[1] >= 3:
res.setProperties(properties.flatten())
return res
elif type == 'VolumeGrid':
from klampt import VolumeGrid
assert len(obj) == 3, "VolumeGrid format is (bmin,bmax,values)"
assert len(obj[2].shape) == 3, "VolumeGrid values must be a 3D array"
bmin = obj[0]
bmax = obj[1]
values = obj[2]
res = VolumeGrid()
res.bbox.append(bmin[0])
res.bbox.append(bmin[1])
res.bbox.append(bmin[2])
res.bbox.append(bmax[0])
res.bbox.append(bmax[1])
res.bbox.append(bmax[2])
res.dims.append(values.shape[0])
res.dims.append(values.shape[1])
res.dims.append(values.shape[2])
vflat = values.flatten()
res.values.resize(len(vflat))
for i, v in enumerate(vflat):
res.values[i] = v
return res
elif type == 'Group':
from klampt import Geometry3D
res = Geometry3D()
assert isinstance(
obj,
(list, tuple)), "Group format is a list or tuple of Geometry3D's"
for i in range(len(obj)):
res.setElement(i, from_numpy(obj[i], 'Geometry3D'))
return res
elif type == 'Geometry3D':
from klampt import Geometry3D
if not isinstance(obj, (list, tuple)) or len(obj) != 2:
raise ValueError("Geometry3D must be a (transform,geometry) tuple")
T = from_numpy(obj[0], 'RigidTransform')
geomdata = obj[1]
subtype = None
if template is not None:
subtype = template.type()
if subtype == 'PointCloud':
g = Geometry3D(
from_numpy(geomdata, subtype, template.getPointCloud()))
else:
g = Geometry3D(from_numpy(geomdata, subtype))
g.setCurrentTransform(*T)
return g
subtype = 'Group'
if all(isinstance(v, np.ndarray) for v in geomdata):
if len(geomdata) == 2:
if len(geomdata[0].shape) == 1 and len(geomdata[1].shape) == 2:
subtype = 'Trajectory'
elif len(geomdata[0].shape) == 2 and len(
geomdata[1].shape) == 2 and geomdata[0].shape[
1] == 3 and geomdata[1].shape[1] == 3:
subtype = 'TriangleMesh'
if len(geomdata) == 3:
if geomdata[0].shape == (3, ) and geomdata[1].shape == (3, ):
subtype = 'VolumeGrid'
g = Geometry3D(from_numpy(obj, subtype))
g.setCurrentTransform(*T)
return g
else:
return obj.flatten()