def __init__(self, a_name):
"""
Constructor
:param a_name:
"""
super(Object, self).__init__(time_out=0.1) # Set duration of Watchdog expiry
self._name = ''
self._state = ObjectState()
self._cmd = ObjectCmd()
self._pub = None
self._sub = None
self.pub_flag = True
self._active = False
self._pose_cmd_set = False # Flag to check if a Pose command has been set from the Object
self._wrench_cmd_set = False # Flag to check if a Wrench command has been set from the Object
import rospy
import numpy as np
import sys
from ambf_msgs.msg import ObjectState, WorldState, WorldCmd
from matplotlib import pyplot
import datetime
wCmd = WorldCmd()
wCmd.enable_step_throttling = True
wCmd.step_clock = True
wCmd.n_skip_steps = 1
g1State = ObjectState()
g2State = ObjectState()
wState = WorldState()
global g1D, g2D, enable_collection
class DataCollection():
def __init__(self, name):
self.data_lim = 1000
if len(sys.argv) > 1:
self.data_lim = int(sys.argv[1])
print 'Taking {} data points'.format(sys.argv[1])
self.data = np.zeros(self.data_lim)
self.ctr = 0
self.name = name
self._enable = False
self._mean = 0
self._std_dev = 0
sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages')
import rospy
except Exception as e:
print(e)
pass
import std_msgs.msg
from ambf_msgs.msg import ObjectState, ObjectCmd
# from kuka7DOF_Spatial_Transform_case import *
from InverseDynamicsModule import *
from geometry_msgs.msg import Vector3
# our global variables
state_msg = ObjectState()
active = True
pos = []
# ROS Subscriber callback function
def get_joint_values(data):
global state_msg, active, pos
pos = data.joint_positions
pos=np.asarray(pos)
# print(pos)
# print "state: ", currentState
return pos
def main():
# \author <http://www.aimlab.wpi.edu>
# \author <*****@*****.**>
# \author Adnan Munawar
# \version 0.1
# */
# //==============================================================================
from ambf_msgs.msg import ObjectState, ObjectCmd
import rospy
from geometry_msgs.msg import Pose
from PyKDL import Rotation, Frame, Vector
global openhmd_state, occulus_state, occulus_state_valid, openhmd_state_valid
openhmd_state = Pose()
occulus_state = ObjectState()
occulus_cmd = ObjectCmd()
occulus_state_valid = False
openhmd_state_valid = False
def openhmd_cb(msg):
global openhmd_state, openhmd_state_valid
openhmd_state = msg
openhmd_state_valid = True
def occulus_cb(msg):
global occulus_state, occulus_state_valid
occulus_state = msg
occulus_state_valid = True
