def __init__(self):
# Generic configuration
np.set_printoptions(precision=4, suppress=True)
# Read configuration parameters
self.ns = rospy.get_namespace()
robot_name = self.ns.replace('/','')
if len(robot_name) == 0:
rospy.logerr('Namespace is required by this script.')
robot_name = 'right'
js_rate = read_parameter('/%s/joint_state_controller/publish_rate' % robot_name, 125.0)
# Populate the OpenRAVE environment and robot
rospack = rospkg.RosPack()
models_dir = rospack.get_path('denso_openrave')
# load yaml
yaml_path = models_dir + '/worlds/stefan_chair_env.yaml'
envdict = yaml.load(file(yaml_path, 'r'))['environment']
openRave = YamlEnv(envdict)
# Get the model
right = openRave.models['right']
bars = add_connecting_bars(openRave)
# enable OpenRave GUI
env = openRave.get_environment()
env.SetViewer('QtCoin')
# Attach kinematics manipulator
rave = Manipulator(env, right, 'denso_ft_sensor_handle')
rave.EnableCollisionChecker()
RaveSetDebugLevel(DebugLevel.Fatal)
# Denso Controllers
denso = 0
#~ denso = JointPositionController(robot_name)
#~ rave.SetDOFValues(denso.get_joint_positions())
rate = rospy.Rate(js_rate)
# Subscribe to FT sensor
#~ rospy.Subscriber('/%s/ft_sensor/raw' % robot_name, WrenchStamped, self.ft_sensor_cb)
#~ rospy.Subscriber('/%s/ft_sensor/diagnostics' % robot_name, DiagnosticArray, self.diagnostics_cb)
#~ rospy.loginfo('Waiting for [/%s/ft_sensor/] topics' % robot_name)
#~ while not rospy.is_shutdown():
#~ rospy.sleep(0.1)
#~ if hasattr(self, 'wrench') and hasattr(self, 'ft_status'):
#~ break
#~ rospy.loginfo('Succefully connected to [/%s/ft_sensor/] topics' % robot_name)
# Move to first pre-contact position
tdelta = 1/js_rate
t = 0.0
tend = 5.0
q = np.array([0.,0.,0.,0.,0.,0.])
while (t<tend):
q[1] = t / tend * np.pi/3
q[2] = t / tend * np.pi/6
rave.SetDOFValues(q)
#~ denso.set_joint_positions(q)
t += tdelta
rate.sleep()
# compute the current data here
while(True):
