def findModelLoader():
try:
return rtm.findObject("ModelLoader")
except:
return None
def findModelLoader(self):
try:
return rtm.findObject("ModelLoader")
except:
return None
# connect between joystick and controller
rtm.connectPorts(js.port('Axes'), midc.port('axes'))
rtm.connectPorts(js.port('Buttons'), midc.port('buttons'))
# connect between controller and PD controller
#rtm.connectPorts(rh.port('angles'), pdc.port('angleRef'))
js.start()
midc.start()
fk.start()
seq.start()
sh.start()
seqsvc.setJointAngles(actual, 0.01)
modelloader = rtm.findObject('ModelLoader')
robot = modelloader.loadBodyInfo(ROBOT_URL)
simfactory = rtm.findObject('DynamicsSimulatorFactory')
planner = simfactory.create()
planner.registerCharacter("robot", robot)
planner.init(0.001,
OpenHRPOrigin.DynamicsSimulator.RUNGE_KUTTA,
OpenHRPOrigin.DynamicsSimulator.DISABLE_SENSOR)
planner.setGVector([0, 0, 9.8])
planner.setCharacterAllJointModes("robot", OpenHRPOrigin.DynamicsSimulator.HIGH_GAIN_MODE)
rospy.init_node("basic_controls")
server = InteractiveMarkerServer("basic_controls")
position = Point( 0, 0, 0)
imarker = make6DofMarker(position)
workmarker1 = makeXYZMarker(Point(0, 1.5, 0))
