def runOneArm(self, arm):
arm.obj_gui.App.update()
# Move the Target Position Based on the GUI
x = arm.obj_gui.x
y = arm.obj_gui.y
z = arm.obj_gui.z
ro = arm.obj_gui.ro
pi = arm.obj_gui.pi
ya = arm.obj_gui.ya
gr = arm.obj_gui.gr
arm.target.set_pos(x, y, z)
arm.target.set_rpy(ro, pi, ya)
p = arm.base.get_pos()
q = arm.base.get_rot()
P_b_w = Vector(p.x, p.y, p.z)
R_b_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
T_b_w = Frame(R_b_w, P_b_w)
p = arm.target.get_pos()
q = arm.target.get_rot()
P_t_w = Vector(p.x, p.y, p.z)
R_t_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
T_t_w = Frame(R_t_w, P_t_w)
T_t_b = T_b_w.Inverse() * T_t_w
# P_t_b = T_t_b.p
# P_t_b_scaled = P_t_b / self.psm1_scale
# T_t_b.p = P_t_b_scaled
computed_q = compute_IK(T_t_b)
# print('SETTING JOINTS: ')
# print(computed_q)
arm.base.set_joint_pos('baselink-yawlink', computed_q[0])
arm.base.set_joint_pos('yawlink-pitchbacklink', computed_q[1])
arm.base.set_joint_pos('pitchendlink-maininsertionlink', computed_q[2])
arm.base.set_joint_pos('maininsertionlink-toolrolllink', computed_q[3])
arm.base.set_joint_pos('toolrolllink-toolpitchlink', computed_q[4])
arm.base.set_joint_pos('toolpitchlink-toolyawlink', -computed_q[5])
arm.base.set_joint_pos('toolyawlink-toolgripper1link', gr)
arm.base.set_joint_pos('toolyawlink-toolgripper2link', gr)
for i in range(3):
if arm.sensor.is_triggered(i) and gr <= 0.2:
sensed_obj = arm.sensor.get_sensed_object(i)
if sensed_obj == 'Needle':
if not arm.grasped[i]:
arm.actuators[i].actuate(sensed_obj)
arm.grasped[i] = True
print('Grasping Sensed Object Names', sensed_obj)
else:
if gr > 0.2:
arm.actuators[i].deactuate()
arm.grasped[i] = False
def main():
global openhmd_state, occulus_state, openhmd_state_valid, occulus_state_valid
rospy.init_node('occulus_view')
openhmd_sub = rospy.Subscriber("/openhmd/pose", Pose, openhmd_cb)
occulus_sub = rospy.Subscriber("/ambf/env/openhmd/State", ObjectState, occulus_cb, queue_size=1)
occulus_pub = rospy.Publisher("/ambf/env/openhmd/Command", ObjectCmd, queue_size=1)
rate = rospy.Rate(60)
counter = 0
start = rospy.get_time()
_first = True
openhmd_initial_rot = Rotation()
occulus_initial_rot = Rotation()
R_pre = Rotation()
R_aInr_offset = Rotation().RPY(0, -1.57079, -1.57079)
# open
while not rospy.is_shutdown():
if openhmd_state_valid and occulus_state_valid:
if _first:
_first = False
openhmd_initial_rot = Rotation.Quaternion(openhmd_state.orientation.x,
openhmd_state.orientation.y,
openhmd_state.orientation.z,
openhmd_state.orientation.w)
occulus_initial_rot = Rotation.Quaternion(occulus_state.pose.orientation.x,
occulus_state.pose.orientation.y,
occulus_state.pose.orientation.z,
occulus_state.pose.orientation.w)
R_pre = openhmd_initial_rot * R_aInr_offset * occulus_initial_rot.Inverse()
else:
occulus_cmd.pose.position = occulus_state.pose.position
openhmd_rot = Rotation.Quaternion(openhmd_state.orientation.x,
openhmd_state.orientation.y,
openhmd_state.orientation.z,
openhmd_state.orientation.w)
delta_rot = R_pre.Inverse() * openhmd_rot * R_aInr_offset
# delta_rot = openhmd_rot
occulus_cmd.pose.orientation.x = delta_rot.GetQuaternion()[0]
occulus_cmd.pose.orientation.y = delta_rot.GetQuaternion()[1]
occulus_cmd.pose.orientation.z = delta_rot.GetQuaternion()[2]
occulus_cmd.pose.orientation.w = delta_rot.GetQuaternion()[3]
occulus_cmd.enable_position_controller = True
occulus_pub.publish(occulus_cmd)
counter = counter + 1
if counter % 60 == 0:
print "- Publishing Occulus Pose ", format( round(rospy.get_time() - start, 3)), 's'
rate.sleep()
def pose_cb(data):
global trans
p = data.pose
trans = Frame(
Rotation.Quaternion(p.orientation.x, p.orientation.y, p.orientation.z,
p.orientation.w),
Vector(p.position.x, p.position.y, p.position.z))
def _update_camera_pose(self):
if self._pose_changed is True:
p = self.camera_handle.get_pos()
q = self.camera_handle.get_rot()
P_c_w = Vector(p.x, p.y, p.z)
R_c_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
self._T_c_w = Frame(R_c_w, P_c_w)
self._T_w_c = self._T_c_w.Inverse()
self._pose_changed = False
def _update_base_pose(self):
if not self._base_pose_updated:
p = self.base.get_pos()
q = self.base.get_rot()
P_b_w = Vector(p.x, p.y, p.z)
R_b_w = Rotation.Quaternion(q.x, q.y, q.z, q.w)
self._T_b_w = Frame(R_b_w, P_b_w)
self._T_w_b = self._T_b_w.Inverse()
self._base_pose_updated = True
def pose_cb(self, msg):
self._pose = msg
self._frame.p = Vector(self._pose.pose.position.x,
self._pose.pose.position.y,
self._pose.pose.position.z)
self._frame.M = Rotation.Quaternion(self._pose.pose.orientation.x,
self._pose.pose.orientation.y,
self._pose.pose.orientation.z,
self._pose.pose.orientation.w)
def pose_msg_to_kdl_frame(msg_pose):
pose = msg_pose.pose
f = Frame()
f.p[0] = pose.position.x
f.p[1] = pose.position.y
f.p[2] = pose.position.z
f.M = Rotation.Quaternion(pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
return f
def pose_msg_to_kdl_frame(msg_pose):
pose = msg_pose.transform
f = Frame()
f.p[0] = pose.translation.x
f.p[1] = pose.translation.y
f.p[2] = pose.translation.z
f.M = Rotation.Quaternion(pose.rotation.x, pose.rotation.y,
pose.rotation.z, pose.rotation.w)
return f
def hydra_msg_to_kdl_frame(msg_hydra):
pose = msg_hydra.paddles[0].transform
f = Frame()
f.p[0] = 10*pose.translation.x
f.p[1] = 10*pose.translation.y
f.p[2] = 10*pose.translation.z
f.M = Rotation.Quaternion(pose.rotation.x,
pose.rotation.y,
pose.rotation.z,
pose.rotation.w)
return f
def pose_to_frame(pose):
# type: (object) -> Frame
if isinstance(pose, PoseStamped):
pose = pose.pose
assert isinstance(pose, Pose)
pos = Vector(pose.position.x, pose.position.y, pose.position.z)
rot = Rotation.Quaternion(pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
frame = Frame(rot, pos)
return frame
def _getOrientation(self, data):
q = Quaternion()
if data:
global hydro
if hydro:
if 'theta' in data:
# Assume Yaw Orientation
orientation = Rotation.RotZ(data.get('theta'))
elif any(k in ['roll', 'pitch', 'yaw']
for k in data.iterkeys()):
# Assume RPY Orientation
orientation = Rotation.RPY(data.get('roll', 0),
data.get('pitch', 0),
data.get('yaw', 0))
elif any(k in ['w', 'x', 'y', 'z'] for k in data.iterkeys()):
orientation = Rotation.Quaternion(data.get('x', 0),
data.get('y', 0),
data.get('z', 0),
data.get('w', 0))
else:
orientation = Rotation()
orientation = orientation.GetQuaternion()
else:
if 'theta' in data:
# Assume Yaw Orientation
orientation = quaternion_from_euler(
0, 0, data.get('theta'))
elif any(k in ['roll', 'pitch', 'yaw']
for k in data.iterkeys()):
# Assume RPY Orientation
orientation = quaternion_from_euler(
data.get('roll', 0), data.get('pitch', 0),
data.get('yaw', 0))
elif any(k in ['w', 'x', 'y', 'z'] for k in data.iterkeys()):
orientation = (data.get('x', 0), data.get('y', 0),
data.get('z', 0), data.get('w', 0))
else:
orientation = (0, 0, 0, 0)
q.x, q.y, q.z, q.w = orientation
return q
def odom_callback(msg):
odom_pose_x = msg.pose.pose.position.x
odom_pose_y = msg.pose.pose.position.y
qx = msg.pose.pose.orientation.x
qy = msg.pose.pose.orientation.y
qz = msg.pose.pose.orientation.z
qw = msg.pose.pose.orientation.w
r = Rotation.Quaternion(qx, qy, qz, qw)
(roll, pitch, odom_pose_r) = r.GetRPY()
world.shared_resource_lock.acquire()
world.odom_pose = [odom_pose_x, odom_pose_y, odom_pose_r]
world.move_speed = [
msg.twist.twist.linear.x, msg.twist.twist.linear.y,
msg.twist.twist.angular.z
]
if world.move_speed[0] < 0.01 and world.move_speed[
1] < 0.01 and world.move_speed[2] < 0.01:
world.rob_is_stop = True
else:
world.rob_is_stop = False
world.odom_pose_time = msg.header.stamp.to_sec()
world.shared_resource_lock.release()
def __init__(self, arm_name):
pose_str = '/dvrk/' + arm_name + '/position_cartesian_current'
wrench_str = '/dvrk/' + arm_name + '/set_wrench_body'
gripper_str = '/dvrk/' + arm_name + '/state_gripper_current'
status_str = '/dvrk/' + arm_name + '/status'
self._mtm_arm_type = None
if arm_name == 'MTMR':
self._mtm_arm_type = 0
elif arm_name == 'MTML':
self._mtm_arm_type = 1
else:
print('SPECIFIED ARM: ', arm_name)
print('WARNING, MTM ARM TYPE NOT UNDERSTOOD, SHOULD BE MTMR or MTML')
pass
self.pose = Frame()
self.pose.p = Vector(0, 0, 0)
self.pose.M = Rotation.Quaternion(0, 0, 0, 1)
self.buttons = 0
self._commanded_force = Vector(0, 0, 0)
self._gripper_min_angle = -3.16
self._gripper_max_angle = 1.2
self._gripper_angle = JointState()
self._gripper_angle.position.append(0)
self._pose_pub = rospy.Publisher(pose_str, PoseStamped, queue_size=1)
self._gripper_pub = rospy.Publisher(gripper_str, JointState, queue_size=1)
self._status_pub = rospy.Publisher(status_str, Empty, queue_size=1)
self._force_sub = rospy.Subscriber(wrench_str, Wrench, self.force_cb, queue_size=10)
self._foot_pedal = ProxyButtons()
pass
# #High point
# psm3.move(PyKDL.Frame(fix_orientation, Vector(0.20207338, -0.08238340, 0.04522971)))
# time.sleep(sleep_time)
# #Point 3
# psm3.move(PyKDL.Frame(fix_orientation, Vector(0.25669699, -0.17719280, 0.05745484)))
# time.sleep(sleep_time)
# #High point
# psm3.move(PyKDL.Frame(fix_orientation, Vector(0.20207338, -0.08238340, 0.04522971)))
# time.sleep(sleep_time)
## Movements with full frames (position + orientation)
fix_orientation = Rotation.Quaternion(
0.26282424,
-0.12377510,
0.49116142,
0.82119644,
)
#Point 1
psm3.move(
PyKDL.Frame(fix_orientation,
Vector(0.22415668, -0.16345158, 0.04202124)))
time.sleep(sleep_time)
# #Point 2
# psm3.move(PyKDL.Frame(fix_orientation, Vector(0.21758554, -0.16898800, 0.04562245)))
# time.sleep(sleep_time)
# #Point 3
# psm3.move(PyKDL.Frame(fix_orientation, Vector(0.20161770, -0.09351339, 0.05061632)))
# time.sleep(sleep_time)
#dmove and Rotation movements didn't seem to work very well
# Retrieve start and end velocity and angular velocity values of tracking frame wrt reference frame
(lin_twist_start,
ang_twist_start) = listener.lookupTwist(reference_frame,
tracking_frame,
start_time,
rospy.Duration(0.1))
(lin_twist_end,
ang_twist_end) = listener.lookupTwist(reference_frame,
tracking_frame, end_time,
rospy.Duration(0.1))
# print "start quat is ", start_quat
# Create matrices for calculation of velocity which can be given into the custom function
start_mat_vel = Frame(
Rotation.Quaternion(start_quat[0], start_quat[1],
start_quat[2], start_quat[3]),
Vector(start_pos[0], start_pos[1], start_pos[2]))
end_mat_vel = Frame(
Rotation.Quaternion(end_quat[0], end_quat[1], end_quat[2],
end_quat[3]),
Vector(end_pos[0], end_pos[1], end_pos[2]))
# Create matrices for calculation of acceleration which can be given into the custom function
start_mat_acc = Frame(
Rotation.Quaternion(start_quat[0], start_quat[1],
start_quat[2], start_quat[3]),
Vector(lin_twist_start[0], lin_twist_start[1],
lin_twist_start[2]))
end_mat_acc = Frame(
Rotation.Quaternion(end_quat[0], end_quat[1], end_quat[2],
end_quat[3]),
def get_box_rot():
br = box.get_rot()
return Rotation.Quaternion(br.x, br.y, br.z, br.w)
def cs_cb(self, msg):
self._cart_state = msg
self._ee_rot = Rotation.Quaternion(msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z,
msg.pose.orientation.w)
cur_time = rospy.Time.now().to_sec()
torque = Vector(0, 0, 0)
last_torque = Vector(0, 0, 0)
d_torque = Vector(0, 0, 0)
# Main Loop
while not rospy.is_shutdown():
App.update()
last_time = cur_time
cur_time = rospy.Time.now().to_sec()
dt = cur_time - last_time
if dt < 0.001:
dt = 0.001
if active:
cur_rot = Rotation.Quaternion(state_msg.pose.orientation.x,
state_msg.pose.orientation.y,
state_msg.pose.orientation.z,
state_msg.pose.orientation.w)
cur_pos = Vector(state_msg.pose.position.x,
state_msg.pose.position.y,
state_msg.pose.position.z)
cmd_pos = Vector(PU.x, PU.y, PU.z)
cmd_rot = Rotation.RPY(PU.roll, PU.pitch, PU.yaw)
last_pos = cur_pos
last_delta_pos = delta_pos
delta_pos = cmd_pos - cur_pos
delta_delta_pos = (delta_pos - last_delta_pos) / dt
# print delta_pos, last_delta_pos
# print (D_lin * delta_delta_pos) / dtp
force = PU.K_lin * delta_pos + PU.D_lin * delta_delta_pos
"""
This script broadcasts transformations from Optitrack
to Baxter coordinate frame. Values for T_HB are constant.
How to get those numbers is explained in README.md
"""
import rospy
import tf
from PyKDL import Vector, Frame, Rotation
if __name__ == '__main__':
rospy.init_node('BaxterTF')
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rate = rospy.Rate(50)
while not rospy.is_shutdown():
T_HB_p = Vector(-0.0924028561603, -0.0172428611168, -0.944187986747)
T_HB_Q = Rotation.Quaternion(-0.00993130, 0.00080579, -0.00441977, 0.99994059) # qx,qy,qz,qw
T_HB = Frame(T_HB_Q, T_HB_p)
T_empty_p = Vector(0, 0, 0)
T_empty_Q = Rotation.Quaternion(0, 0, 0, 1)
T_empty = Frame(T_empty_Q, T_empty_p)
# Sending new transformations
br.sendTransform(T_HB.p, T_HB.M.GetQuaternion(), rospy.Time.now(), 'base', 'bax_head')
br.sendTransform(T_HB.p, T_HB.M.GetQuaternion(), rospy.Time.now(), 'reference/base', 'bax_head')
br.sendTransform(T_empty.p, T_empty.M.GetQuaternion(), rospy.Time.now(), 'world', 'base')
rate.sleep()
try:
(trans_OH,
rot_OH) = listener.lookupTransform('/optitrack', '/bax_head',
rospy.Time(0))
(trans_OA,
rot_OA) = listener.lookupTransform('/optitrack', '/bax_arm',
rospy.Time(0))
(trans_BG,
rot_BG) = listener.lookupTransform('/base', '/left_gripper_base',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
# Rotations
rot_OH = Rotation.Quaternion(*rot_OH)
rot_OA = Rotation.Quaternion(*rot_OA)
rot_BG = Rotation.Quaternion(*rot_BG)
rot_AG = Rotation.RPY(math.pi / 2, -math.pi, math.pi / 2)
# Creating Frames
T_OH = Frame(rot_OH, Vector(*trans_OH))
T_OA = Frame(rot_OA, Vector(*trans_OA))
T_BG = Frame(rot_BG, Vector(*trans_BG))
T_AG = Frame(rot_AG, Vector(0, 0, 0))
# Finding right transformation
T_HB = T_OH.Inverse() * T_OA * T_AG * T_BG.Inverse()
T_empty_p = Vector(0, 0, 0)
T_empty_Q = Rotation.Quaternion(0, 0, 0, 1)
rospy.init_node('baxter_optitrack_transformer')
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rate = rospy.Rate(50.0)
while not rospy.is_shutdown():
# Transformation -------------------------------------------------------------
#T_GB_p=Vector(0.0707,0.0187,-0.8593) # project
#T_GB_Q=Rotation.Quaternion(-0.004002,-0.004752,0.999994,-0.000251)
#T_GB=Frame(T_GB_Q,T_GB_p) [0.101118 <-0.573142, 0.049050, 0.811713>] (0.421538700623461, 0.5060896478392994, 0.5964256398717394, -0.45875358127230415)
T_GB_p = Vector(-0.0157712999511, -0.835218066682,
-0.0674771192051) #INKA
T_GB_Q = Rotation.Quaternion(-0.484148, -0.486831, -0.522866,
0.505181) # qz,qx,qy,qw INKA
#T_GB_p=Vector(0.0366584397092, -0.839661563598, -0.202769519381) #JA
#T_GB_Q=Rotation.Quaternion(0.421538700623461, 0.5060896478392994, 0.5964256398717394, -0.45875358127230415) # qz,qx,qy,qw JA
T_GB = Frame(T_GB_Q, T_GB_p)
T_empty_p = Vector(0, 0, 0)
T_empty_Q = Rotation.Quaternion(0, 0, 0, 1)
T_empty = Frame(T_empty_Q, T_empty_p)
Rotx_p = Vector(0, 0, 0)
Rotx_Q = Rotation.Quaternion(0.70682518, 0, 0, 0.70682518) # x za 90
Rotx = Frame(Rotx_Q, Rotx_p)
Roty_p = Vector(0, 0, 0)
Roty_Q = Rotation.Quaternion(0, 0.70682518, 0, 0.70738827) # y za 90
