Go to the documentation of this file.00001 import math
00002 from javax.swing import *
00003 from java.awt import *
00004 import hrp
00005 from OpenHRP.StateHolderServicePackage import CommandHolder
00006
00007 class jointPanel(JPanel):
00008 def __init__(self, linkinfo, seq, angle):
00009 self.seq = seq
00010 if len(linkinfo.ulimit) > 0:
00011 self.maxDeg = linkinfo.ulimit[0]*180/math.pi
00012 else:
00013 self.maxDeg = 180
00014 if len(linkinfo.llimit) > 0:
00015 self.minDeg = linkinfo.llimit[0]*180/math.pi
00016 else:
00017 self.minDeg = -180
00018 self.jointId = linkinfo.jointId
00019 self.name = linkinfo.name
00020 nameLabel = JLabel(linkinfo.name+":")
00021 nameLabel.setPreferredSize(Dimension(150, 15));
00022 self.add(nameLabel)
00023 self.add(JLabel("%4.0f"%self.minDeg))
00024 self.slider = JSlider(maximum=int(self.maxDeg), minimum=int(self.minDeg), stateChanged=self.setAngleSlider)
00025 self.add(self.slider)
00026 self.add(JLabel("%4.0f"%self.maxDeg))
00027 self.text = JTextField(5, actionPerformed=self.setAngleText)
00028 self.add(self.text)
00029 self.angle = angle
00030 def setAngleText(self, event):
00031 txt = self.text.getText()
00032 angDeg = float(txt)
00033 if angDeg > self.maxDeg:
00034 angDeg = self.maxDeg
00035 self.text.setText(str(angDeg))
00036 if angDeg < self.minDeg:
00037 angDeg = self.minDeg
00038 self.text.setText(str(angDeg))
00039 self.slider.setValue(int(float(angDeg)))
00040 self.setTargetAngle(angDeg)
00041 def setAngleSlider(self, event):
00042 angDeg = self.slider.getValue()
00043 self.text.setText(str(angDeg))
00044 self.setTargetAngle(angDeg)
00045 def setCurrentAngle(self,angle):
00046 self.slider.setValue(int(angle))
00047 self.angle = angle
00048 def setTargetAngle(self, target):
00049 delta = abs(self.angle - target)
00050 if delta == 0:
00051 return
00052 tm = delta/20.0
00053 if tm < 0.1:
00054 tm = 0.1
00055 self.seq.waitInterpolation()
00056 self.seq.setJointAngle(self.name, target*math.pi/180, tm)
00057 self.angle = target
00058
00059
00060 class poseEditor(JFrame):
00061 def __init__(self, url, seq, sh):
00062 ml = hrp.findModelLoader()
00063 self.bodyInfo = ml.getBodyInfo(url)
00064 self.dof = 0
00065 for li in self.bodyInfo.links():
00066 if li.jointId >= 0:
00067 self.dof += 1
00068 print "dof=",self.dof
00069 self.setSize(550, 800)
00070 self.seq = seq
00071 self.sh = sh
00072 panel = JPanel()
00073 panel.setLayout(BorderLayout())
00074 sPane = ScrollPane()
00075 sPane.setSize(400,720)
00076 jointsPanel = JPanel()
00077 jointsPanel.setLayout(BoxLayout(jointsPanel,
00078 BoxLayout.Y_AXIS))
00079 command = CommandHolder()
00080 self.sh.getCommand(command)
00081 jav = command.value.jointRefs
00082 self.jpanels = []
00083 for li in self.bodyInfo.links():
00084 if li.jointId >= 0:
00085 jp = jointPanel(li, seq, jav[li.jointId]*180/math.pi)
00086 self.jpanels.append(jp)
00087 jointsPanel.add(jp)
00088 sPane.add(jointsPanel)
00089 panel.add(sPane, BorderLayout.NORTH)
00090 commandPanel = JPanel()
00091 commandPanel.add(JButton("get pose", actionPerformed=self.getPose))
00092 panel.add(commandPanel, BorderLayout.SOUTH)
00093 self.add(panel)
00094 self.getPose(None)
00095 self.setVisible(True)
00096 def getPose(self, event):
00097 command = CommandHolder()
00098 self.sh.getCommand(command)
00099 jav = command.value.jointRefs
00100 for jp in self.jpanels:
00101 jp.setCurrentAngle(jav[jp.jointId]*180/math.pi)
00102
00103
00104
00105
00106