Your browser was unable to load all of the resources. They may have been blocked by your firewall, proxy or browser configuration.
Press Ctrl+F5 or Ctrl+Shift+R to have your browser try again.

update a rigid body in a visualization through user input #54

#1

Hello, I want to do something pretty simple: rotate a rigid body given 3 user provided angles (roll pitch, yaw).
Currently, I have a Qt window and a visualization side by side.
Desired behavior: The user enters the yaw in the Qt window, rigid body yaws. User enters roll, rigid body rolls, etc.
Current behavior: no communication between the Qt window and the visualization thread: user enters a value in the Qt window, and nothing happens.
Can someone tell me how to update the visualization with values gotten from the PyQt thread?
Note: the below code block doesn't work.
Thank you

from klampt import *
import sys
from PyQt5.QtWidgets import QApplication, QMainWindow, QVBoxLayout, QHBoxLayout, QLineEdit, QPushButton, QLabel
from PyQt5.QtWidgets import  QWidget, QSplitter, QFrame
from numpy import cos, arccos, sin, arcsin, tan, arctan, deg2rad, radians

class RotationWindow(QMainWindow):
    def __init__(self, klamptGLWindow, world):
        super().__init__()
        self.world = world
        self.sim = Simulator(self.world)
        self.splitter = QSplitter()
        self.left = QFrame()
        self.leftLayout = QVBoxLayout()
        self.initLeftLayout()
        self.left.setLayout(self.leftLayout)
        self.right = QFrame()
        self.rightLayout = QVBoxLayout()
        self.right.setLayout(self.rightLayout)

        self.glwidget = klamptGLWindow
        self.glwidget.setParent(self.right)
        self.rightLayout.addWidget(self.glwidget)
        self.splitter.addWidget(self.left)
        self.splitter.addWidget(self.right)
        self.splitter.setHandleWidth(7)
        self.setCentralWidget(self.splitter)
    

    def initLeftLayout(self):
        # central_widget = QWidget(self)
        # self.setCentralWidget(central_widget)
        # self.leftLayout = QVBoxLayout(central_widget)

        # QLineEdit fields and corresponding labels
        self.roll_lineedit, self.roll_label = self.create_labeled_lineedit("Roll")
        self.pitch_lineedit, self.pitch_label  = self.create_labeled_lineedit("Pitch")
        self.yaw_lineedit, self.yaw_label  = self.create_labeled_lineedit("Yaw")

        # Connect textChanged signals to callback methods
        self.roll_lineedit.textChanged.connect(self.on_roll_text_changed)
        self.pitch_lineedit.textChanged.connect(self.on_pitch_text_changed)
        self.yaw_lineedit.textChanged.connect(self.on_yaw_text_changed)

        # QHBoxLayout for each row (label + line edit + button)
        self.roll_layout = self.create_row_layout("Apply Roll Rotation", self.roll_label, self.roll_lineedit, self.applyRollRotation)
        self.pitch_layout = self.create_row_layout("Apply Pitch Rotation", self.pitch_label, self.pitch_lineedit, self.applyPitchRotation)
        self.yaw_layout = self.create_row_layout("Apply Yaw Rotation", self.yaw_label, self.yaw_lineedit, self.applyYawRotation)

        # Adding layouts to the main ayout
        self.leftLayout.addLayout(self.roll_layout)
        self.leftLayout.addLayout(self.pitch_layout)
        self.leftLayout.addLayout(self.yaw_layout)

        self.setWindowTitle('Rotation Window')
        self.show()
    
    def create_labeled_lineedit(self, label_text):
        label = QLabel(label_text)
        line_edit = QLineEdit(self)
        return line_edit, label

    def create_row_layout(self, button_text, label, line_edit, pureRotation):
        row_layout = QHBoxLayout()

        # QLabel
        row_layout.addWidget(label)

        # QLineEdit
        row_layout.addWidget(line_edit)

        # QPushButton
        button = QPushButton(button_text, self)
        button.clicked.connect(lambda _, le=line_edit: pureRotation(le))
        row_layout.addWidget(button)

        return row_layout

    def applyRollRotation(self, line_edit):
        theta = float(line_edit.text())
        print(f"Applying Roll of {theta} degrees")
        
        M1 = [1,        0,             0 ,
              0,        cos(theta),    sin(theta),
              0,        sin(theta),    cos(theta)]
        self.world.rigidObject(0).setTransform(M1, [0,0,0])
        sim.simulate(0.01)
        self.sim.updateWorld()
        
        

    def applyPitchRotation(self, line_edit):
        theta = float(line_edit.text())
        print(f"Applying Pitch of {theta} degrees")
        M2 = [cos(theta), 0, -sin(theta) ,
              0,              1,   0,
              sin(theta),  0, cos(theta)]
        self.world.rigidObject(0).setTransform(M2, [0,0,0])
        sim.simulate(0.01)
        self.sim.updateWorld()
        


    def applyYawRotation(self, line_edit):
        theta = float(line_edit.text())
        print(f"Applying Yaw Rotation of {theta} degrees")
        M3 = [cos(theta), sin(theta),  0,
            -sin(theta), cos(theta),  0,
            0,              0,              1]
        self.world.rigidObject(0).setTransform(M3, [0,0,0])
        sim.simulate(0.01)
        self.sim.updateWorld()



    def on_roll_text_changed(self, text):
        print(f"Roll: {text}")

    def on_pitch_text_changed(self, text):
        print(f"Pitch: {text}")

    def on_yaw_text_changed(self, text):
        print(f"Yaw: {text}")

if __name__ == '__main__':
    app = QApplication(sys.argv)
    window = RotationWindow()
    sys.exit(app.exec_())
#!/usr/bin/python3
from klampt import *
from klampt.model.trajectory import RobotTrajectory
import time
import os
KLAMPT_ROOT = f"{os.getcwd()}" #root contains the data directory
KLAMPT_WS = f"{KLAMPT_ROOT}/Python3"
from numpy import cos, arccos, sin, arcsin, tan, arctan, deg2rad, radians

import QtMainWindowForRPYRotation as QtRotationWindow

def makefunc(gl_backend): 
    global g_mainwindow
    g_mainwindow = QtRotationWindow.RotationWindow(gl_backend)
    return g_mainwindow


if __name__ == "__main__":
    #some Klamp't code
    w = WorldModel()
    ss = w.loadRigidObject(f"{KLAMPT_WS}/RahelsPrograms/spaceShuttle.stl") #return a RigidObjectModel
    
    if ss == None:
        print("loading spaceShip.stl failed ")
    
    else:
        vis.customUI(makefunc)
        vis.add("world",w)
        vis.add("ss", ss)
        vis.edit("ss")
        
        # from klampt import Simulator
        # sim = Simulator(w)
        # def setup():
        #     vis.show()
        # def callback():
        #     ss = sim.body(w.rigidObject(0))
        #     '''
        #     by default, the simulator controls the world dynamics. 
        #     to control the dynamics ourselves, we need to disable 
        #     the simulator from doing so. 
        #     '''
        #     ss.enableDynamics(False) 

        #     ''' we need to set the velocity inside the loop, 
        #     not just in setup(). 
        #     if the velocity (or other dynamics) are not controlled
        #     this causes a runtime error  that the Rigid object energy
        #     exceeds the maximum energy threshold.
        #     here, the velocity is controlled manually but 
        #     it can be controlled via PIDs '''
        #     # ss.setVelocity([0,0,0.000],[0.000,0,0])
        #     # sim.simulate(0.01)
        #     # sim.updateWorld()
        #     theta = radians(30)
        #     R3 = [cos(theta), sin(theta), 0,\
        #          -sin(theta), cos(theta), 0,\
        #          0          ,0          , 1]
        #     translation = [0,0,0]
        #     w.rigidObject(0).setTransform(R3, translation)

        #vis.loop(setup=setup,callback=callback)
        vis.loop()
Screen Shot 2023-12-16 at 6.12.28 AM.png
  • replies 0
  • views 608
  • likes 0