Ejemplo n.º 1
0
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)
Ejemplo n.º 2
0
    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))
Ejemplo n.º 3
0
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)
Ejemplo n.º 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)
Ejemplo n.º 8
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)
Ejemplo n.º 9
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)
Ejemplo n.º 10
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], [[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
Ejemplo n.º 11
0
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)
Ejemplo n.º 13
0
 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'
Ejemplo n.º 14
0
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)
Ejemplo n.º 15
0
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)
Ejemplo n.º 16
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)
Ejemplo n.º 17
0
 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)
Ejemplo n.º 19
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)
Ejemplo n.º 20
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
Ejemplo n.º 21
0
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)
Ejemplo n.º 22
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
Ejemplo n.º 23
0
    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)
Ejemplo n.º 24
0
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)
Ejemplo n.º 25
0
    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])
Ejemplo n.º 26
0
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])
Ejemplo n.º 27
0
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]
Ejemplo n.º 28
0
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))
Ejemplo n.º 29
0
 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))
Ejemplo n.º 30
0
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()