def __init__(self):
# Variables
self.contact = False
self.first_config = False
self.curr = np.array([0.,0.,0.,0.,0.,0.])
self.curr_buff = np.array([0.,0.,0.,0.,0.,0.])
self.pos_buff = np.array([0.,0.,0.,0.,0.,0.])
K_mot = np.array([0.,1.122861,0.695228,0.,0.,0.]) # Change K
self.param = np.array([[0.,0.,0.,0.,0.,0.],[0.,0.,0.,0.,0.,0.],[0.,0.,0.,0.,0.,0.]]) # initial is 0
self.z = np.array([0.,0.,0.,0.,0.,0.]) # initial is 0
self.time = [0]
self.qdd = np.array([0.,0.,0.,0.,0.,0.])
# 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:
#~ robot_name = 'right'
rospy.logerr('Namespace is required by this script.')
return
js_rate = read_parameter('/%s/joint_state_controller/publish_rate' % robot_name, 125.0)
self.dt = 1. / js_rate
# Populate the OpenRAVE environment and robot
rospack = rospkg.RosPack()
models_dir = rospack.get_path('denso_openrave')
env = Environment()
denso_base = env.ReadRobotXMLFile(models_dir + '/objects/denso_base.kinbody.xml')
denso_base.SetName('denso_base')
env.Add(denso_base)
robot = env.ReadRobotXMLFile(models_dir + '/robots/denso_ft_sensor_handle.robot.xml')
robot.SetName(robot_name)
env.Add(robot)
self.rave = Manipulator(env, robot, '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())
self.rate = rospy.Rate(js_rate)
# Subscribe to FT sensor and joint state
rospy.Subscriber('/%s/ft_sensor/raw' % robot_name, WrenchStamped, self.ft_sensor_cb)
rospy.Subscriber('/%s/joint_states' % robot_name, JointState, self.joint_states_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') and hasattr(self, 'effort') and hasattr(self, 'position'):
break
rospy.loginfo('Succefully connected to [/%s/ft_sensor/] topics' % robot_name)
rospy.loginfo('Succefully connected to [/%s/joint_states/] topics' % robot_name)
# Move to first pre-contact position
Tprecontact = tr.euler_matrix(0, -np.pi/2 - np.pi/4 + 10/180.0*np.pi, 0)
Tprecontact[:3,3] = np.array([0.3, 0., -0.018])
self.move_robot(Tprecontact)
# Move to second pre-contact position
Tprecontact[0,3] -= 0.01
self.move_robot(Tprecontact)
rospy.sleep(0.5)
# Move until contact is happening (speed = 0.25 cm/s)
q = self.rave.GetDOFValues()
J = np.zeros((6,6))
curr_ref = self.curr[-1]
dx = np.array([-0.002*self.dt, 0., 0., 0., 0., 0.])
dq_buf = np.array([0.,0.,0.,0.,0.,0.])
Wref = np.array(self.wrench)
contact_force = np.array([1.0,10.0,5.0,0.0]),num = 0
while not rospy.is_shutdown():
if self.ft_status == DiagnosticStatus().OK:
J[:3,:] = rave.manipulator.CalculateJacobian()
J[3:,:] = rave.manipulator.CalculateAngularVelocityJacobian()
dq = np.dot(np.linalg.pinv(J), dx)
self.qdd = (dq - dq_buf) / self.dt / self.dt
dq_buf = dq
q += dq
self.rave.SetDOFValues(q)
self.rave.SetDOFVelocities(dq/self.dt)
self.denso.set_joint_positions(q)
self.rate.sleep()
# Calculate F/T
Tor = []
for (i,Ki) in enumerate(K_mot):
Tor.append(Ki*(self.curr[i] - curr_ref[i]) + self.param[2,i] + self.z[i])
global_ft_calc = - np.dot(np.linalg.pinv(J.T), Tor)
T = self.rave.GetEndEffectorTransform(self.rave.GetDOFValues())
force_frame_transform
bXeF = force_frame_transform(T)
local_ft_calc = np.dot(np.linalg.pinv(bXeF), global_ft_calc)
print 'local_ft_calc: '
for (i,loc_i) in enumerate(local_ft_calc):
print('%f ' %loc_i)
print '\n'
# FT sensor
W = np.array(self.wrench)
F = W[:3] - Wref[:3]
if ((np.linalg.norm(F) > contact_force[num]) and (num<2)) or ((np.linalg.norm(F) < contact_force[num]) and (num>=2)):
rospy.loginfo('Contact %d detected' %(num+1))
if(num==0):
self.contact = True
self.first_config = True
num +=1
if(num==2):
dx[0] = -dx[0]
rospy.loginfo('Last local_ft_calc:')
local_ft_calc
rospy.loginfo('Last F:')
F
rospy.loginfo('Move to next contact point')
rospy.sleep(3)
continue
else:
rospy.loginfo('FT Sensor failed')
J[:3,:] = rave.manipulator.CalculateJacobian()
J[3:,:] = rave.manipulator.CalculateAngularVelocityJacobian()
global_ft_calc.append(np.linalg.solve(J.T, Tor))
ext_jnt_torque.append(np.array(Tor))
elif topic.find('/ft_sensor/raw') >= 0:
force = np.array([msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z])
torque = np.array([msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z])
ft_queue.append(-np.hstack((force,torque)))
if (len(ft_queue) < js_window) or not js_alive:
continue
wrench_queue = ft_filter(np.array(ft_queue))
#~ wrench_queue = np.array(ft_queue)
T = rave.GetEndEffectorTransform(jnt_position[-1])
force_frame_transform
bXeF = force_frame_transform(T)
global_wrench = np.dot(bXeF, wrench_queue[-1])
time_ft_list.append(t.to_sec())
time_ft = np.array(time_ft_list[:]) - time_ft_list[0]
if(time_ft[-1] < t_precon):
wrench_offset = global_wrench
global_wrench = np.array([0.,0.,0.,0.,0.,0.])
else:
global_wrench -= wrench_offset
ext_tor.append(np.dot(J.T, global_wrench))
wrench.append(global_wrench)
bag.close()
wrench = np.array(wrench)
mot_torque = np.array(mot_torque)
