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:
robot_name = 'right'
rospy.logerr('Namespace is required by this script.')
self.js_rate = read_parameter('/%s/joint_state_controller/publish_rate' % robot_name, 250.0)
# Create low pass filter
order = read_parameter('~lowpass_filter_order', 3)
cutoff = read_parameter('~lowpass_cutoff', 5)
self.js_window = int(self.js_rate/5)
self.js_filter = ButterLowPass(cutoff, self.js_rate, order=order)
self.mot_tor_filter = ButterLowPass(1.0, self.js_rate, order=5)
self.js_queue = collections.deque(maxlen=self.js_window )
self.mot_tor_queue = collections.deque(maxlen=self.js_window )
self.mot_torque_avg = collections.deque(maxlen=self.js_window )
self.mot_torque = collections.deque(maxlen=self.js_window )
self.jnt_position = collections.deque(maxlen=self.js_window )
self.Tmcg = collections.deque(maxlen=(self.js_window) )
# Variables
self.contact = False
self.param = np.array([[0.989438, 1.146991, 0.501995, 0.173604, 0.246897, 0.083080],[0.656342, 0.710369, 0.352390, -0.407913, 0.181837, -0.136770],[0.076302, 0.024945, 0.069612, 0.957069, 0.032772, 0.523249]])
self.z = np.array([10.340966, 16.791938, 13.832300, -1.771149, 12.406588, 8.797445])
self.Tor = np.array([0.,0.,0.,0.,0.,0.])
self.i = 1
self.k = 0
# 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)
self.rate = rospy.Rate(self.js_rate)
# Subscribe to joint state
rospy.Subscriber('/%s/joint_states' % robot_name, JointState, self.joint_states_cb)
rospy.loginfo('Waiting for [/%s/joint_states] topics' % robot_name)
self.js_alive = False
while not (self.js_alive):
rospy.sleep(0.1)
if rospy.is_shutdown():
return
rospy.loginfo('Succefully connected to [/%s/joint_states/] topics' % robot_name)
while(True):
continue
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):
def __init__(self, sim=False):
""" Constructor
Construct all 3D world's information. Then constructs both planner and
executor object.
"""
# Get parameters
robot_name = read_parameter('~robot_name', 'left')
table_id = int(read_parameter('~table_id', 2))
# Init openrave env
self.env = Environment()
if sim:
self.env.SetViewer('qtcoin')
# Get the robot
rospack = rospkg.RosPack()
path = rospack.get_path('light_drawing') + '/robots/denso_light.robot.xml'
self.robot = self.env.ReadRobotXMLFile(path)
self.robot.SetName(robot_name)
manip_name = 'light'
# Get the objects
table_path = rospack.get_path('denso_openrave') + '/objects/table.kinbody.xml'
table = self.env.ReadKinBodyXMLFile(table_path)
Ttable = np.eye(4)
Ttable[:3, 3] = np.array([0.7, 0.3, 0.126])
# Ttable[2, 3] += 9.0e-3 # Uncomment if using the acrylic printing board
table.SetTransform(Ttable)
# Add the objects to the environment.
self.env.AddRobot(self.robot)
self.env.AddKinBody(table)
self.sim = sim
# Init planner and executor
self.pl = PrintPlanner(self.env, robot_name, manip_name, hardware=False)
self.ex = DensoRobot(self.env, robot_name, manip_name)
self.ex.ScaleVelocityLimits(1)
self.ex.ScaleAccelerationLimits(1)
# Advertise the service
srv_name = 'draw_light'
s = rospy.Service(srv_name, DrawingRequest, self.handle_request)
rospy.loginfo("Service %s is ready" % srv_name)
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')
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())
denso = 0
self.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
q = self.rave.GetDOFValues()
q[4] = np.pi/2
q[2] = np.pi/6
Tprecontact = self.rave.GetEndEffectorTransform(q)
self.move_robot(Tprecontact)
while not rospy.is_shutdown():
continue
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')
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')
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())
denso = 0
self.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)
#~
joint_num = 2
a0 = np.pi/6
a = np.pi/65
freq = 0.3
w = 2.0*np.pi* freq
dt = 1./js_rate
t = 0.
q = self.rave.GetDOFValues()
#~ q[0] = np.pi/2
q[1] = np.pi/2
#~ q[2] = np.pi/2 - q[1]
self.move_robot(self.rave.GetEndEffectorTransform(q))
#~ while(q[2]<np.pi/2):
#~ q[2] += 0.1*dt
#~ q[3] += 0.1*dt
#~ self.rave.SetDOFValues(q)
#~ self.denso.set_joint_positions(q)
#~ self.rate.sleep()
rospy.sleep(5.0)
q = self.rave.GetDOFValues()
a1 = 5./180.*np.pi
f0 = 0.1
B = 0.05
while(t < 5./freq):
q[joint_num] += a1*np.cos(2*np.pi*(f0*t+B*t*t))*2*np.pi*(f0+2*B*t)*dt
#~ q[joint_num] += (a*np.sin(w*t) + w*(a0+a*t)*np.cos(w*t))*dt
self.rave.SetDOFValues(q)
#~ self.denso.set_joint_positions(q)
self.rate.sleep()
t+=dt
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')
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)
rave = Manipulator(env, robot, 'denso_ft_sensor_handle')
rave.EnableCollisionChecker()
RaveSetDebugLevel(DebugLevel.Fatal)
# enable OpenRave GUI
env.SetViewer('QtCoin')
# Denso Controllers
#~ denso = JointPositionController(robot_name)
#~ rave.SetDOFValues(denso.get_joint_positions())
denso = 0
rate = rospy.Rate(js_rate)
# Move to first pre-contact position
Tprecontact = tr.euler_matrix(0, -np.pi/2 - np.pi/4, 0)
Tprecontact[:3,3] = np.array([0.3, 0., -0.025])
self.move_robot(denso, rave, Tprecontact, rate)
# Move to second pre-contact position
Tprecontact[0,3] -= 0.01
self.move_robot(denso, rave, Tprecontact, rate)
rospy.sleep(0.5)
# 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 until contact is happening (speed = 0.25 cm/s)
q = rave.GetDOFValues()
dt = 1. / js_rate
dx = np.array([-0.0025*dt, 0, 0])
#~ Wref = np.array(self.wrench)
while not rospy.is_shutdown():
if True or self.ft_status == DiagnosticStatus().OK:
J = rave.manipulator.CalculateJacobian()
dq = np.dot(np.linalg.pinv(J), dx)
q += dq
rave.SetDOFValues(q)
#~ denso.set_joint_positions(q)
rate.sleep()
# FT sensor
#~ W = np.array(self.wrench)
#~ F = W[:3] - Wref[:3]
if (rave.GetEndEffectorTransform(q)[0,3] < 0.25):#(np.linalg.norm(F) > 1.):
rospy.loginfo('Contact detected')
break
else:
rospy.loginfo('FT Sensor failed')
# Apply sinusoidal motion profile using jacobian velocity
#~ x(t) = A * math.sin(w*t - (np.pi / 2.))
for freq in np.array([0.1 , 0.2 , 0.3 , 0.4, 0.5]): # 0.01 , 0.03 , 0.1 , 0.3, 1.0
A = 0.001 # 0.028 degree
w = 2.*np.pi*freq
t = 0
rospy.loginfo('Applying motion profile with freq %f Hz' %freq)
while (t < 4./freq) and not rospy.is_shutdown():
if True or self.ft_status == DiagnosticStatus().OK:
J = rave.manipulator.CalculateJacobian()
dq = -w * A*math.cos(w*t - (np.pi / 2.)) * dt
q[4] += dq
#~ denso.set_joint_positions(q)
rate.sleep()
rave.SetDOFValues(q)
t += dt
else:
rospy.loginfo('FT Sensor failed')
rospy.loginfo('Finished motion profile freq %f Hz' %freq)
rospy.sleep(1)
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)
mot_tor_filter = ButterLowPass(1.0, js_rate, order=5)
# Process rosbag data
directory = os.path.expanduser('~/data/rosbag_data/')
for bag_file in os.listdir(directory):
if not (os.path.splitext(bag_file)[0] == node_name):
continue
bag = rosbag.Bag(os.path.join(directory, bag_file))
bag_info = yaml.load(bag._get_yaml_info())
# The two topics must have the same number of messages
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)
def __init__(self):
# Generic configuration
np.set_printoptions(precision = 4, suppress=True)
# Read configuration parameters
left_robot_name = read_parameter('~left_robot_name', 'left')
self.js_rate = read_parameter('/%s/joint_state_controller/publish_rate' % left_robot_name, 125.0)
self.table_id = int(read_parameter('~table_id', 2))
self.stick_id = int(read_parameter('~stick_id', 3))
# Populate the OpenRAVE environment
rospack = rospkg.RosPack()
models_dir = rospack.get_path('denso_openrave')
env = Environment()
left_robot = env.ReadRobotXMLFile(models_dir + '/robots/denso_ft_gripper_with_base.robot.xml')
left_robot.SetName('left_robot')
table = env.ReadKinBodyXMLFile(models_dir + '/objects/table.kinbody.xml')
pin = env.ReadKinBodyXMLFile(models_dir + '/objects/pin.kinbody.xml')
wood_stick = env.ReadKinBodyXMLFile(models_dir + '/objects/wood_stick.kinbody.xml')
env.Add(left_robot)
env.Add(table)
env.Add(pin)
env.Add(wood_stick)
self.left_rave = Manipulator(env, left_robot, 'denso_ft_sensor_gripper')
#Change the limits of the gripper in the model so that the model in openrave can close until touch the pin
[lowerlimits, upperlimits] = left_robot.GetDOFLimits()
upperlimits[-1] = np.deg2rad(43)
left_robot.SetDOFLimits(lowerlimits, upperlimits)
RaveSetDebugLevel(DebugLevel.Fatal)
# Place elements in OpenRAVE using TF information
self.tf_updater = TFtoOpenRAVE(env, self.global_frame)
self.tf_updater.add_body('%s/base_link' % left_robot_name, left_robot, duration=5.)
self.tf_updater.add_body('rigid_body_%d' % self.table_id, table, duration=5.)
self.tf_updater.add_body('rigid_body_%d' % self.stick_id, wood_stick)
self.tf_updater.start()
env.SetViewer('QtCoin')
# Denso Controllers
self.left_denso = JointPositionController(left_robot_name)
self.left_rave.SetDOFValues(self.left_denso.get_joint_positions())
self.left_rave.EnableCollisionChecker()
rospy.Subscriber('/optitrack_rigid_bodies', RigidBodyArray, self.optitrack_cb)
rospy.loginfo('Reading object poses from optitrack...')
#Set the pin into the position-in openrave env (grasped by the gripper)
q_denso = self.left_denso.get_joint_positions()
T_denso = self.left_rave.GetEndEffectorTransform(q_denso)
c90 = np.cos(np.pi/2)
s90 = np.sin(np.pi/2)
T_pininhand = np.array(
[[c90,-s90, 0, 0.015],
[s90, c90, 0, 0 ],
[0 ,0 , 1, 0.005 ],
[0 ,0 , 0, 1 ]])
pin.SetTransform(np.dot(T_denso, T_pininhand))
self.left_rave.CloseGripper()
self.left_rave.robot.WaitForController(5)
with self.left_rave.env:
self.left_rave.robot.Grab(pin)
# Position-based explicit force control
self.state_alive = False
rospy.Subscriber('/%s/endpoint_state' % left_robot_name, EndpointState, self.endpoint_state_cb)
rospy.loginfo('Waiting for [/%s/endpoint_state] topic' % left_robot_name)
self.state_alive = False
while not self.state_alive and not rospy.is_shutdown():
rospy.sleep(0.1)
rospy.Subscriber('/%s/ft_sensor/diagnostics' % left_robot_name, DiagnosticArray, self.diagnostics_cb)
rospy.loginfo('Waiting for [/%s/ft_sensor/] topics' % left_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' % left_robot_name)
# Real code that make the robot move
exploration_data = []
# TODO: identify 3 points of 3 planes to explore (will be developed further, using region growing or planar detector..)
point1 = np.array([0,0,0,1])
direction = np.array([0,0,-1])
ex1 = [point1,direction]
exploration_data.append(ex1)
point2 = np.array([-0.03,-0.01,-0.05,1])
direction2 = np.array([1,0,0])
ex2 = [point2,direction2]
exploration_data.append(ex2)
point3 = np.array([0., -0.01,-0.05,1])
direction3 = np.array([0,1,0])
ex3 = [point3,direction3]
exploration_data.append(ex3)
#TODO: Estimate the pin position
do_the_pin_estimation = raw_input('Press E if you want to estimate the pin position ')
if do_the_pin_estimation == 'e' or do_the_pin_estimation == 'E':
estimate_hand2pin()
else:
self.T_hand2pin = np.eye(4)
self.T_hand2pin[:3,3] = np.array([-0.03,0,-0.015])
# TODO: Collect measurements
o_p = 2e-3
o_n = 15/180.0*np.pi
Tws = wood_stick.GetTransform()
Tws[:3,3] = Tws[:3,3] + np.array([0.00,0.0,0.017])
collecteddata = []
self.forcelist = []
for i in range(3):
waitkey = raw_input("Press Enter to collect new data point...")
num_points = 0
point = np.dot(Tws,exploration_data[i][0])
direction = np.dot(Tws[:3,:3],exploration_data[i][1])
touched = self.touch_surface(point[:3], direction, 0.4 , 0.03)
if not((touched==np.zeros(3)).all()):
num_points +=1
d = [touched, direction, o_p,o_n]
collecteddata.append(d)
print "Found ", num_points, " point(s) on the surface\n"
print "New point found: ", touched ,"\n"
else:
print "Not touched! Moving to another exploration position"
while num_points < 2:
#TODO: plan new point
waitkey = raw_input("Press Enter to continue...")
radius = np.random.uniform(0.01,0.015)
rand_perpendicular_vector = perpendicular_vector(direction)/np.linalg.norm(perpendicular_vector(direction))
new_point = point[:3] + radius*rand_perpendicular_vector
new_direction = direction
#TODO: approach and touch
touched = self.touch_surface(new_point, new_direction, 0.4 , 0.03)
#TODO: update num_points
if not((touched==np.zeros(3)).all()):
num_points +=1
d = [touched, new_direction, o_p,o_n]
collecteddata.append(d)
print "Found ", num_points, " point(s) on the surface\n"
print "New point found: ", touched ,"\n"
else:
print "Not touched! Moving to another exploration position"
if rospy.is_shutdown():
return
rospy.loginfo("COLLECT MEASUREMENT DONE")
print collecteddata
# TODO: Insert the pin into the hole
rospy.loginfo("Move the pin to the correct position and start inserting")
delta0 = 22
dim = 6 # 6 DOFs
ptcl0 = ParticleFilterLib.Particle(Tws[:3,3], tr.euler_from_matrix(Tws[:3,:3]))
V0 = ParticleFilterLib.Region([ptcl0], delta0)
M = 6 # No. of particles per delta-neighbohood
delta_desired = 0.9 # Terminal value of delta
woodstick = env.ReadKinBodyXMLFile('wood_stick.xml')
env.Add(woodstick,True)
env.Remove(wood_stick)
list_particles, weights = ParticleFilterLib.ScalingSeries(V0, collecteddata, M, delta0, delta_desired, dim, body = woodstick, env = env)
est = ParticleFilterLib.VisualizeParticles(list_particles, weights, env= env, body=woodstick, showestimated = True)
# print "Real transf. ", Tws
# self.touch_surface(est[:3,3], direction, 0.4 , 0.03)
rospy.loginfo("DONE")
