~csiro-asl/csiro-asl-ros-drivers/trunk

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
#! /usr/bin/python

# 
# CSIRO Autonomous Systems Laboratory
# Queensland Centre for Advanced Technologies
# PO Box 883, Kenmore, QLD 4069, Australia
# http://www.ict.csiro.au/
#  
# Copyright (c) CSIRO 2010
# 
# Nick Hillier
# 
# Simple script to test physics plugins
#

import roslib

roslib.load_manifest('odephysics_ORplugin')

from openravepy import *

import signal
def signal_handler(signal, frame):
    exit()
signal.signal(signal.SIGINT, signal_handler)

cont_type = 'multi'#'odevel'

# initialise OpenRave
dt = 0.005 # Simulation Timestep
env = Environment()
# optionally attach viewer
env.SetViewer('qtcoin') # attaching a viewer seems to stuff up the timesteps, so we set them AFTER attaching the viewer
env.StopSimulation()
# load an xml file to be the environment
fname = roslib.packages.get_pkg_dir('odephysics_ORplugin')+'/test/testphysics2.env.xml'
env.Load(fname) 
# get a reference to the test robot
robots = env.GetRobots()
testbot = []
for i in range(len(robots)):
    if robots[i].GetName() == 'testbot': # the name is specified in xml file
        testbot = robots[i]
        break
# setup controller
if cont_type == 'odevel':
    cont = RaveCreateController(env,"csiro_odevelocity")
    #cont = RaveCreateController(env,"odevelocity")
else:
    cont = RaveCreateController(env,"csiro_combinedcontroller")
testbot.SetController(cont)
#start simulation
with env:
    env.StartSimulation(timestep=dt,realtime=True) # limits the simulation to running at, or slower, than realtime.

body = env.ReadKinBodyXMLFile('data/mug1.kinbody.xml')
body.SetName('mug')

from numpy import *
with env:
    env.AddKinBody(body)
    T = eye(4)
    T[0:3,3] = array((-0.5,-0.5,2.5))
    body.SetTransform(T)
    
import time
arm = 1.0
wheel = 1.0
dn = 1.0
while(1):
    dn = -dn
    if cont_type == 'odevel':
        string = 'setvelocity %f %f %f %f %f %f'%(dn*arm, -dn*arm, dn*wheel, dn*wheel, -dn*wheel, -dn*wheel) # all velocity controlled
        cont.SendCommand(string)
    elif cont_type == 'multi':
        cont.SetDesired(([dn*arm, -dn*arm, 5.0*dn*wheel, 5.0*dn*wheel, -dn*wheel, -dn*wheel])) # 2 arm positions, then 4 wheel vels
    # else: no control input
    time.sleep(5)