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):
# Initialize node
optimize_part = False # True : optimize the graph
testing = False # True: to test the parameter with actual data, False : Search for optimization
node_name = 'data_13' # Connected to the corresponding rosbag
sample_n = 1 # change the sample in here (must be equal or less than 'sample' in param.yaml)
rospy.init_node(node_name)
rospy.loginfo('Starting [%s] node' % node_name)
js_rate = 250.0
rate = rospy.Rate(js_rate)
# Load denso_openrave path
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 environment
env = openRave.get_environment()
#~ env.SetViewer('QtCoin')
# Add right arm
right = openRave.models['right']
rave = Manipulator(env, right, 'denso_ft_sensor_handle')
rave.EnableCollisionChecker()
RaveSetDebugLevel(DebugLevel.Fatal)
# Initialize Filter parameters
np.set_printoptions(precision=4, suppress=True)
order = read_parameter('~lowpass_filter_order', 3)
cutoff = read_parameter('~lowpass_cutoff', 5)
js_window = int(js_rate/5)
js_filter = ButterLowPass(cutoff, js_rate, order=order)
ft_filter = ButterLowPass(cutoff, js_rate, order=order)
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'
#~ return
js_rate = read_parameter('/%s/joint_state_controller/publish_rate' % robot_name, 250.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 environment
env = openRave.get_environment()
# Add right arm
right = openRave.models['right']
self.rave = Manipulator(env, right, 'denso_ft_sensor_handle')
self.rave.EnableCollisionChecker()
RaveSetDebugLevel(DebugLevel.Fatal)
# enable OpenRave GUI
env.SetViewer('QtCoin')
# Denso Controllers
#~ self.denso = JointPositionController(robot_name)
#~ self.rave.SetDOFValues(self.denso.get_joint_positions())
denso = 0
self.rate = rospy.Rate(js_rate)
# Move to first pre-contact position
q = self.rave.GetDOFValues()
T0 = self.rave.GetEndEffectorTransform(q) # get end effector
T0[0:3,3] += [0.255, 0.23, 0.0]
T0[2,3] = 0.034
Tprecon = tr.euler_matrix(0, np.pi, np.pi/2)
Tprecon[:3,3] = T0[:3,3]
self.move_robot(Tprecon)
# subscribe to force_state
#~ rospy.sleep(1.0)
#~ rospy.Subscriber('/%s/force_state' % robot_name, ForceState, self.est_force_cb)
#~ rospy.loginfo('Waiting for [/%s/force_state/] topics' % robot_name)
#~ while not rospy.is_shutdown():
#~ rospy.sleep(0.1)
#~ if hasattr(self, 'wrench'):
#~ break
#~ rospy.loginfo('Succefully connected to [/%s/force_state/] topics' % robot_name)
raw_input('enter to continue')
rospy.loginfo('Moving to contact')
q = self.rave.GetDOFValues()
dt = 1./js_rate
dx = np.array([0., 0., -0.0005*dt, 0., 0., 0.])
J = np.zeros((6,6))
#~ Wref = np.array(self.wrench)
while not rospy.is_shutdown():
J[:3,:] = self.rave.manipulator.CalculateJacobian()
J[3:,:] = self.rave.manipulator.CalculateAngularVelocityJacobian()
dq = np.linalg.solve(J, dx)
q += dq
self.rave.SetDOFValues(q)
#~ denso.set_joint_positions(q)
self.rate.sleep()
#~ W = np.array(self.wrench)
#~ F = W[:3] - Wref[:3]
if (np.linalg.norm(F) > 20.):
rospy.loginfo('Contact detected')
break
rospy.loginfo('stay for a while....')
rospy.sleep(5.0)
raw_input('enter to start force control')
# Start force control
q = self.rave.GetDOFValues()
Fcontrol = 20.0
Kp = 0.00001
Kv = 0.0001
Fe = collections.deque(maxlen=10)
timeout = 30
initime = rospy.get_time
while not rospy.is_shutdown() and (rospy.get_time()-initime)<timeout:
F = W[2] - Wref[2]
Fe.append(Fcontrol - F)
if(len(Fe)<10):
continue
dFe = np.diff(np.array(Fe), n=1, axis=0)* js_rate
dx[2] = Kp*np.array(Fe)[-1] + Kv*dFe[-1]
J[:3,:] = self.rave.manipulator.CalculateJacobian()
J[3:,:] = self.rave.manipulator.CalculateAngularVelocityJacobian()
dq = np.linalg.solve(J, dx)
q += dq
self.rave.SetDOFValues(q)
#~ denso.set_joint_positions(q)
self.rate.sleep()
t += dt
rospy.loginfo('Finish the experiment....')
rospy.sleep(5.0)
self.move_robot(Tprecon)
