def main(unused_argv):
vrb = ViveRobotBridge()
# vrb.__init__()
rospy.init_node('vive_listener')
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
# pub = rospy.Publisher('tarpos_pub', geometry_msgs.msg.Transform, queue_size=1)
# _joy_sub = rospy.Subscriber('/vive/controller_LHR_FFFF3F47/joy', Joy, vive_controller_button_callback, queue_size=1)
temp_flag = 0
pre_position = [0, 0, 0]
pre_pose = [1, 0, 0, 0]
temp = [1, 0, 0, 0]
rate = rospy.Rate(100.0)
limits = 0.02
env = Environment(FLAGS.assets_root,
disp=FLAGS.disp,
shared_memory=FLAGS.shared_memory,
hz=480)
task = tasks.names[FLAGS.task]()
task.mode = FLAGS.mode
agent = task.oracle(env)
env.set_task(task)
obs = env.reset()
info = None
ee_pose = ((0.46562498807907104, -0.375, 0.3599780201911926), (0.0, 0.0,
0.0, 1.0))
while not rospy.is_shutdown():
#act = agent.act(obs, info)
#obs, reward, done, info = env.step(act)
env.movep(ee_pose)
if vrb.grasp == 1:
env.ee.activate()
else:
env.ee.release()
try:
trans = tfBuffer.lookup_transform('world',
'controller_LHR_FF777F05',
rospy.Time())
# rospy.loginfo(trans)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.loginfo("error")
rate.sleep()
continue
if vrb.offset_flag == 1:
if temp_flag == 0:
# vrb.offset[0] = trans.transform.translation.x
# vrb.offset[1] = trans.transform.translation.y
# vrb.offset[2] = trans.transform.translation.z
# print(vrb.offset)
pre_position[0] = trans.transform.translation.x
pre_position[1] = trans.transform.translation.y
pre_position[2] = trans.transform.translation.z
pre_pose[0] = trans.transform.rotation.x
pre_pose[1] = trans.transform.rotation.y
pre_pose[2] = trans.transform.rotation.z
pre_pose[3] = trans.transform.rotation.w
else:
msg = geometry_msgs.msg.Transform()
# msg.translation.x = (trans.transform.translation.x-pre_position[0])/10
# msg.translation.y = (trans.transform.translation.y-pre_position[1])/10
# msg.translation.z = (trans.transform.translation.z-pre_position[2])/10
# compute delta distance
msg.translation.x = trans.transform.translation.x - pre_position[
0]
msg.translation.y = trans.transform.translation.y - pre_position[
1]
msg.translation.z = trans.transform.translation.z - pre_position[
2]
# if msg.translation.x >0.02:
# msg.translation.x = 0.02
# if msg.translation.y >0.02:
# msg.translation.y = 0.02
# if msg.translation.z >0.02:
# msg.translation.z = 0.02
# if msg.translation.x <-0.02:
# msg.translation.x = -0.02
# if msg.translation.y <-0.02:
# msg.translation.y = -0.02
# if msg.translation.z <-0.02:
# msg.translation.z = -0.02
if msg.translation.x > limits:
msg.translation.x = limits
if msg.translation.y > limits:
msg.translation.y = limits
if msg.translation.z > limits:
msg.translation.z = limits
if msg.translation.x < -limits:
msg.translation.x = -limits
if msg.translation.y < -limits:
msg.translation.y = -limits
if msg.translation.z < -limits:
msg.translation.z = -limits
print(msg.translation)
# temp[0] = trans.transform.rotation.x
# temp[1] = trans.transform.rotation.y
# temp[2] = trans.transform.rotation.z
# temp[3] = trans.transform.rotation.w
#
#
#
# q = Quaternion(pre_pose) * Quaternion(temp).inverse
#
#
# msg.rotation.x = q.x
# msg.rotation.y = q.y
# msg.rotation.z = q.z
# msg.rotation.w = q.w#
msg.rotation.x = trans.transform.rotation.x
msg.rotation.y = trans.transform.rotation.y
msg.rotation.z = trans.transform.rotation.z
msg.rotation.w = trans.transform.rotation.w
ee_position = list(p.getLinkState(env.ur5, env.ee_tip)[4])
#ee_orientation = p.getLinkState(self.ur5, self.ee_tip)[5]
ee_orientation = (0, 0, 0, 1)
ee_position[0] = ee_position[0] + msg.translation.y
ee_position[1] = ee_position[1] - msg.translation.x
# z axis limit
z_control = ee_position[2] + msg.translation.z
if z_control < 0.02:
z_control = 0.02
ee_position[2] = z_control
ee_pose = (tuple(ee_position), ee_orientation)
# rectified quaternion
# theta_x = np.arcsin(2*(trans.transform.rotation.w*trans.transform.rotation.y
# - trans.transform.rotation.z*trans.transform.rotation.x))
# temp = (trans.transform.rotation.x**2 + trans.transform.rotation.z**2)**0.5
# z_v = trans.transform.rotation.z / temp
# x_v = trans.transform.rotation.x / temp
# msg.rotation.x = x_v * np.sin(theta_x)#0#np.sin(0.5*theta_x)
# msg.rotation.y = 0#y_v * np.sin(theta_x)#0
# msg.rotation.z = z_v * np.sin(theta_x)
# msg.rotation.w = trans.transform.rotation.w
#
# msg.translation.x = 0
# msg.translation.y = 0
# msg.translation.z = 0
#
# msg.rotation.x = 0
# msg.rotation.y = 0
# msg.rotation.z = 0
# msg.rotation.w = 1
print(msg.rotation)
vrb.pub.publish(msg)
pre_position[0] = trans.transform.translation.x
pre_position[1] = trans.transform.translation.y
pre_position[2] = trans.transform.translation.z
temp_flag = vrb.offset_flag
rate.sleep()
def main(unused_argv):
vrb = ViveRobotBridge()
# vrb.__init__()
rospy.init_node('raven_vive_teleop')
temp_flag = 0
pre_position = [0, 0, 0]
pre_pose = [1, 0, 0, 0]
temp = [1, 0, 0, 0]
rate = rospy.Rate(60.0)
limits = 0.02
env = Environment(assets_root, disp=True, hz=60)
task = tasks.names[task_name]()
task.mode = mode
agent = task.oracle(env)
env.set_task(task)
obs = env.reset()
info = None
ee_pose = ((0.46562498807907104, -0.375, 0.3599780201911926), (0.0, 0.0,
0.0, 1.0))
ee_position = [0.0, 0.0, 0.0]
while not rospy.is_shutdown():
""" print(vrb.vive_controller_translation)
print(vrb.offset_flag)
print(vrb.grasp)
continue """
#p.stepSimulation()
#p.getCameraImage(480, 320)
if vrb.trigger_pressed_event == 1:
# get current pose of the end-effector
ee_position_ref = list(p.getLinkState(env.ur5, env.ee_tip)[4])
ee_orientation = p.getLinkState(env.ur5, env.ee_tip)[5]
vrb.trigger_pressed_event = 0
# print("I--trigger pressed!\n")
if vrb.trigger_released_event == 1:
vrb.trigger_released_event = 0
# print("O--trigger released!\n")
if vrb.trigger_current_status == 1:
ee_orientation = vrb.vive_controller_rotation
# ee_orientation = (0,0,0,1)
ee_position[
0] = ee_position_ref[0] + vrb.vive_controller_translation[0]
ee_position[
1] = ee_position_ref[1] + vrb.vive_controller_translation[1]
# z axis limit
z_control = ee_position_ref[2] + vrb.vive_controller_translation[2]
# print("ee position", vrb.vive_controller_translation[1],
# - vrb.vive_controller_translation[0], vrb.vive_controller_translation[2])
if z_control < -0.02:
z_control = 0.02
ee_position[2] = z_control
ee_pose = (tuple(ee_position), ee_orientation)
env.movep(ee_pose)
marker_head_point = [ee_position[0], ee_position[1], 0.05]
marker_tail_point = [ee_position[0], ee_position[1], 0.06]
p.addUserDebugLine(marker_head_point,
marker_tail_point,
lineColorRGB=[1, 0, 0],
lifeTime=0.2,
lineWidth=3)
# if vrb.grasp == 1:
# env.ee.activate()
# else:
# env.ee.release()
#
p.stepSimulation()
rate.sleep()