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 convert_DH_to_ambf_joint(self, a, alpha, d, theta, joint_data):
rot_bINa = Rotation(math.cos(theta), - math.sin(theta) * math.cos(alpha), math.sin(theta) * math.sin(alpha),
math.sin(theta), math.cos(theta) * math.cos(alpha), - math.cos(theta) * math.sin(alpha),
0, math.sin(alpha), math.cos(alpha))
pos_bINa = Vector(a * math.cos(theta), a * math.sin(theta), d)
for i in range(0, 3):
pos_bINa[i] = round(pos_bINa[i], 3)
for j in range(0, 3):
rot_bINa[i, j] = round(rot_bINa[i, j], 3)
trans_bINa = Frame(rot_bINa, pos_bINa)
# The child pivot and axis defined the joint in child frame. So we set
# the parent pivot and axis to zero and default respectively and then
# invert the Trans of B in A to get Trans of A in B and set the child
# pivot and axis based on that
trans_aINb = trans_bINa.Inverse()
print('DH: a: %s alpha: %s d: %s theta: %s', a, alpha, d, theta)
print(rot_bINa)
print(pos_bINa)
print('------')
parent_pivot = {'x': 0, 'y': 0, 'z': 0}
parent_axis = {'x': 0, 'y': 0, 'z': 1}
child_pivot = {'x': round(trans_aINb.p[0], 3),
'y': round(trans_aINb.p[1], 3),
'z': round(trans_aINb.p[2], 3)}
child_axis = {'x': round(trans_aINb.M[0, 2], 3),
'y': round(trans_aINb.M[1, 2], 3),
'z': round(trans_aINb.M[2, 2], 3)}
print(parent_pivot)
print(parent_axis)
print(child_pivot)
print(child_axis)
print('---------')
joint_data['parent axis'] = parent_axis
joint_data['parent pivot'] = parent_pivot
joint_data['child pivot'] = child_pivot
joint_data['child axis'] = child_axis
class PSM:
def __init__(self, client, name):
self.client = client
self.name = name
self.base = self.client.get_obj_handle(name + '/baselink')
self.target_IK = self.client.get_obj_handle(name + '_target_ik')
self.palm_joint_IK = self.client.get_obj_handle(name + '_palm_joint_ik')
self.target_FK = self.client.get_obj_handle(name + '_target_fk')
self.sensor = self.client.get_obj_handle(name + '/Sensor0')
self.actuators = []
self.actuators.append(self.client.get_obj_handle(name + '/Actuator0'))
time.sleep(0.5)
self.grasped = [False, False, False]
self.T_t_b_home = Frame(Rotation.RPY(3.14, 0.0, 1.57079), Vector(0.0, 0.0, -1.0))
# Transform of Base in World
self._T_b_w = None
# Transform of World in Base
self._T_w_b = None
self._base_pose_updated = False
self._num_joints = 6
self._ik_solution = np.zeros([self._num_joints])
self._last_jp = np.zeros([self._num_joints])
def set_home_pose(self, pose):
self.T_t_b_home = pose
def is_present(self):
if self.base is None:
return False
else:
return True
def get_ik_solution(self):
return self._ik_solution
def get_T_b_w(self):
self._update_base_pose()
return self._T_b_w
def get_T_w_b(self):
self._update_base_pose()
return self._T_w_b
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 run_grasp_logic(self, jaw_angle):
for i in range(len(self.actuators)):
if jaw_angle <= 0.2:
if self.sensor is not None:
if self.sensor.is_triggered(i):
sensed_obj = self.sensor.get_sensed_object(i)
if sensed_obj == 'Needle' or 'Thread' in sensed_obj:
if not self.grasped[i]:
qualified_nane = '/ambf/env/BODY ' + sensed_obj
self.actuators[i].actuate(qualified_nane)
self.grasped[i] = True
print('Grasping Sensed Object Names', sensed_obj)
else:
if self.actuators[i] is not None:
self.actuators[i].deactuate()
if self.grasped[i] is True:
print('Releasing Grasped Object')
self.grasped[i] = False
# print('Releasing Actuator ', i)
def move_cp(self, T_t_b):
if type(T_t_b) in [np.matrix, np.array]:
T_t_b = convert_mat_to_frame(T_t_b)
ik_solution = compute_IK(T_t_b)
self._ik_solution = enforce_limits(ik_solution)
self.move_jp(self._ik_solution)
def optimize_jp(self, jp):
# Optimizing the tool shaft roll angle
pass
def move_jp(self, jp):
self.base.set_joint_pos('baselink-yawlink', jp[0])
self.base.set_joint_pos('yawlink-pitchbacklink', jp[1])
self.base.set_joint_pos('pitchendlink-maininsertionlink', jp[2])
self.base.set_joint_pos('maininsertionlink-toolrolllink', jp[3])
self.base.set_joint_pos('toolrolllink-toolpitchlink', jp[4])
self.base.set_joint_pos('toolpitchlink-toolyawlink', jp[5])
def set_jaw_angle(self, jaw_angle):
self.base.set_joint_pos('toolyawlink-toolgripper1link', jaw_angle)
self.base.set_joint_pos('toolyawlink-toolgripper2link', jaw_angle)
def measured_cp(self):
pass
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)
T_empty = Frame(T_empty_Q, T_empty_p)
# Broadcast 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()
class GeomagicDevice:
# The name should include the full qualified prefix. I.e. '/Geomagic/', or '/omniR_' etc.
def __init__(self, name):
pose_topic_name = name + 'pose'
twist_topic_name = name + 'twist'
button_topic_name = name + 'button'
force_topic_name = name + 'force_feedback'
self._active = False
self._scale = 0.0002
self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.twist = PyKDL.Twist()
# This offset is to align the pitch with the view frame
R_off = Rotation.RPY(0.0, 0, 0)
self._T_baseoffset = Frame(R_off, Vector(0, 0, 0))
self._T_baseoffset_inverse = self._T_baseoffset.Inverse()
self._T_tipoffset = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.clutch_button_pressed = False # Used as Position Engage Clutch
self.gripper_button_pressed = False # Used as Gripper Open Close Binary Angle
self._force = DeviceFeedback()
self._force.force.x = 0
self._force.force.y = 0
self._force.force.z = 0
self._force.position.x = 0
self._force.position.y = 0
self._force.position.z = 0
self._pose_sub = rospy.Subscriber(pose_topic_name,
PoseStamped,
self.pose_cb,
queue_size=1)
self._twist_sub = rospy.Subscriber(twist_topic_name,
Twist,
self.twist_cb,
queue_size=1)
###### Commented
self._button_sub = rospy.Subscriber(button_topic_name,
DeviceButtonEvent,
self.buttons_cb,
queue_size=1)
self._force_pub = rospy.Publisher(force_topic_name,
DeviceFeedback,
queue_size=1)
self.switch_psm = False
self._button_msg_time = rospy.Time.now()
self._switch_psm_duration = rospy.Duration(0.5)
print('Creating Geomagic Device Named: ', name, ' From ROS Topics')
self._msg_counter = 0
def set_base_frame(self, frame):
self._T_baseoffset = frame
self._T_baseoffset_inverse = self._T_baseoffset.Inverse()
pass
def set_tip_frame(self, frame):
self._T_tipoffset = frame
pass
def set_scale(self, scale):
self._scale = scale
def get_scale(self):
return self._scale
def pose_cb(self, msg):
cur_frame = pose_msg_to_kdl_frame(msg)
cur_frame.p = cur_frame.p * self._scale
self.pose = self._T_baseoffset_inverse * cur_frame * self._T_tipoffset
# Mark active as soon as first message comes through
self._active = True
pass
def twist_cb(self, msg):
twist = PyKDL.Twist()
twist[0] = msg.linear.x
twist[1] = msg.linear.y
twist[2] = msg.linear.z
twist[3] = msg.angular.x
twist[4] = msg.angular.y
twist[5] = msg.angular.z
self.twist = self._T_baseoffset_inverse * twist
pass
def buttons_cb(self, msg):
self.gripper_button_pressed = msg.white_button
self.clutch_button_pressed = msg.grey_button
if self.clutch_button_pressed:
time_diff = rospy.Time.now() - self._button_msg_time
if time_diff.to_sec() < self._switch_psm_duration.to_sec():
print('Allow PSM Switch')
self.switch_psm = True
self._button_msg_time = rospy.Time.now()
def command_force(self, force):
pass
def measured_cp(self):
return self.pose
def measured_cv(self):
return self.twist
def get_jaw_angle(self):
if self.gripper_button_pressed:
return 0.0
else:
return 0.3
class Camera:
def __init__(self, client, name):
self.client = client
self.name = name
self.camera_handle = self.client.get_obj_handle(name)
time.sleep(0.5)
# Transform of Base in World
self._T_c_w = None
# Transform of World in Base
self._T_w_c = None
self._pose_changed = True
self._num_joints = 6
self._ik_solution = np.zeros([self._num_joints])
self._last_jp = np.zeros([self._num_joints])
def is_present(self):
if self.camera_handle is None:
return False
else:
return True
def get_T_c_w(self):
self._update_camera_pose()
return self._T_c_w
def get_T_w_c(self):
self._update_camera_pose()
return self._T_w_c
def has_pose_changed(self):
return self._pose_changed
def set_pose_changed(self):
self._pose_changed = True
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 move_cp(self, T_c_w):
if type(T_c_w) in [np.matrix, np.array]:
T_c_w = convert_mat_to_frame(T_c_w)
self.camera_handle.set_pos(T_c_w.p[0], T_c_w.p[1], T_c_w.p[2])
rpy = T_c_w.M.GetRPY()
self.camera_handle.set_rpy(rpy[0], rpy[1], rpy[2])
self._pose_changed = True
def move_cv(self, twist, dt):
if type(twist) in [np.array, np.ndarray]:
v = Vector(twist[0], twist[1], twist[2]) * dt
w = Vector(twist[3], twist[4], twist[5]) * dt
elif type(twist) is Twist:
v = twist.vel * dt
w = twist.rot * dt
else:
raise TypeError
T_c_w = self.get_T_c_w()
T_cmd = Frame(Rotation.RPY(w[0], w[1], w[2]), v)
self.move_cp(T_c_w * T_cmd)
pass
def measured_cp(self):
return self.get_T_c_w()
class MTM:
# The name should include the full qualified prefix. I.e. '/Geomagic/', or '/omniR_' etc.
def __init__(self, name):
pose_sub_topic_name = name + 'position_cartesian_current'
twist_topic_name = name + 'twist_body_current'
joint_state_sub_topic_name = name + 'state_joint_current'
gripper_topic_name = name + 'state_gripper_current'
clutch_topic_name = '/dvrk/footpedals/clutch'
coag_topic_name = '/dvrk/footpedals/coag'
pose_pub_topic_name = name + 'set_position_cartesian'
wrench_pub_topic_name = name + 'set_wrench_body'
effort_pub_topic_name = name + 'set_effort_joint'
self.cur_pos_msg = None
self.pre_coag_pose_msg = None
self._active = False
self._scale = 1.0
self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.twist = PyKDL.Twist()
R_off = Rotation.RPY(0, 0, 0)
self._T_baseoffset = Frame(R_off, Vector(0, 0, 0))
self._T_baseoffset_inverse = self._T_baseoffset.Inverse()
self._T_tipoffset = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.clutch_button_pressed = False # Used as Position Engage Clutch
self.coag_button_pressed = False # Used as Position Engage Coag
self.gripper_angle = 0
self._pose_sub = rospy.Subscriber(pose_sub_topic_name,
PoseStamped,
self.pose_cb,
queue_size=1)
self._state_sub = rospy.Subscriber(joint_state_sub_topic_name,
JointState,
self.state_cb,
queue_size=1)
self._gripper_sub = rospy.Subscriber(gripper_topic_name,
JointState,
self.gripper_cb,
queue_size=1)
self._twist_sub = rospy.Subscriber(twist_topic_name,
TwistStamped,
self.twist_cb,
queue_size=1)
self._clutch_button_sub = rospy.Subscriber(clutch_topic_name,
Joy,
self.clutch_buttons_cb,
queue_size=1)
self._coag_button_sub = rospy.Subscriber(coag_topic_name,
Joy,
self.coag_buttons_cb,
queue_size=1)
self._pos_pub = rospy.Publisher(pose_pub_topic_name,
Pose,
queue_size=1)
self._wrench_pub = rospy.Publisher(wrench_pub_topic_name,
Wrench,
queue_size=1)
self._effort_pub = rospy.Publisher(effort_pub_topic_name,
JointState,
queue_size=1)
self.switch_psm = False
self._button_msg_time = rospy.Time.now()
self._switch_psm_duration = rospy.Duration(0.5)
self._jp = []
self._jv = []
self._jf = []
print('Creating MTM Device Named: ', name, ' From ROS Topics')
self._msg_counter = 0
def set_base_frame(self, frame):
self._T_baseoffset = frame
self._T_baseoffset_inverse = self._T_baseoffset.Inverse()
pass
def optimize_wrist_platform(self):
qs = self._jp
vs = self._jv
Kp_4 = 0.3
Kd_4 = 0.03
lim_4 = 0.2
Kp_6 = 0.0
Kd_6 = 0.0
lim_6 = 0.01
# Limits for Wrist Pitch (Joint 5)
a_lim_5 = -1.5
b_lim_5 = 1.2
c_lim_5 = 1.8
sign = 1
if a_lim_5 < qs[4] <= b_lim_5:
sign = 1
elif b_lim_5 < qs[4] < c_lim_5:
range = c_lim_5 - b_lim_5
normalized_val = (qs[4] - b_lim_5) / range
centerd_val = normalized_val - 0.5
sign = -centerd_val * 2
print('MID VAL:', sign)
# sign = 0
else:
sign = -1
e = qs[5]
tau_4 = Kp_4 * e * sign - Kd_4 * vs[3]
tau_4 = np.clip(tau_4, -lim_4, lim_4)
tau_6 = -Kp_6 * qs[5] - Kd_6 * vs[5]
tau_6 = np.clip(tau_6, -lim_6, lim_6)
js_cmd = [0.0] * 7
js_cmd[3] = tau_4
js_cmd[5] = tau_6
self.move_jf(js_cmd)
def set_tip_frame(self, frame):
self._T_tipoffset = frame
pass
def set_scale(self, scale):
self._scale = scale
def get_scale(self):
return self._scale
def pose_cb(self, msg):
self.cur_pos_msg = msg
if self.pre_coag_pose_msg is None:
self.pre_coag_pose_msg = self.cur_pos_msg
cur_frame = pose_msg_to_kdl_frame(msg)
cur_frame.p = cur_frame.p * self._scale
self.pose = self._T_baseoffset_inverse * cur_frame * self._T_tipoffset
# Mark active as soon as first message comes through
self._active = True
pass
def state_cb(self, msg):
self._jp = msg.position
self._jv = msg.velocity
self._jf = msg.effort
pass
def is_active(self):
return self._active
def gripper_cb(self, msg):
min = -1.8
max = 1.4
self.gripper_angle = msg.position[0] + min / (max - min)
pass
def twist_cb(self, msg):
twist = PyKDL.Twist()
omega = msg.twist
twist[0] = omega.linear.x
twist[1] = omega.linear.y
twist[2] = omega.linear.z
twist[3] = omega.angular.x
twist[4] = omega.angular.y
twist[5] = omega.angular.z
self.twist = self._T_baseoffset_inverse * twist
pass
def clutch_buttons_cb(self, msg):
self.clutch_button_pressed = msg.buttons[0]
self.pre_coag_pose_msg = self.cur_pos_msg
if self.clutch_button_pressed:
time_diff = rospy.Time.now() - self._button_msg_time
if time_diff.to_sec() < self._switch_psm_duration.to_sec():
print('Allow PSM Switch')
self.switch_psm = True
self._button_msg_time = rospy.Time.now()
def coag_buttons_cb(self, msg):
self.coag_button_pressed = msg.buttons[0]
self.pre_coag_pose_msg = self.cur_pos_msg
def command_force(self, force):
pass
def move_cp(self, pose):
if type(pose) == PyKDL.Frame:
pose_msg = kdl_frame_to_pose_msg(pose).pose
elif type(pose) == PoseStamped:
pose_msg = pose.pose
else:
pose_msg = pose
self._pos_pub.publish(pose_msg)
def move_cf(self, wrench):
wrench = self._T_baseoffset_inverse * wrench
wrench_msg = kdl_wrench_to_wrench_msg(wrench)
self._wrench_pub.publish(wrench_msg.wrench)
def move_jf(self, torques):
effort_msg = vector_to_effort_msg(torques)
self._effort_pub.publish(effort_msg)
def measured_cp(self):
return self.pose
def measured_jp(self):
return self._jp
def measured_jf(self):
return self._jf
def measured_cv(self):
return self.twist
def get_jaw_angle(self):
return self.gripper_angle
class GeomagicDevice:
# The name should include the full qualified prefix. I.e. '/Geomagic/', or '/omniR_' etc.
def __init__(self, name, mtm_name):
pose_str = name + 'pose'
button_str = name + 'button'
force_str = name + 'force_feedback'
self._active = False
self._scale = 0.0009
self.pose = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.base_frame = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.tip_frame = Frame(Rotation().RPY(0, 0, 0), Vector(0, 0, 0))
self.grey_button_pressed = False # Used as Position Engage Clutch
self.white_button_pressed = False # Used as Gripper Open Close Binary Angle
self._force = DeviceFeedback()
self._force.force.x = 0
self._force.force.y = 0
self._force.force.z = 0
self._force.position.x = 0
self._force.position.y = 0
self._force.position.z = 0
self._pose_sub = rospy.Subscriber(pose_str, PoseStamped, self.pose_cb, queue_size=10)
self._button_sub = rospy.Subscriber(button_str, DeviceButtonEvent, self.buttons_cb, queue_size=10)
self._force_pub = rospy.Publisher(force_str, DeviceFeedback, queue_size=1)
# External Clutch Buttons Requested by ICL
extra_footPedal_str = '/footPedal'
self._extra_pedal_cb = rospy.Subscriber(extra_footPedal_str, Vector3, self.extra_footpedal_cb, queue_size=1)
self._engageMTM = True
self._externalFootPedalMsg = Vector3()
self._geomagicButtonMsg = DeviceButtonEvent()
print('BINDING GEOMAGIC DEVICE: ', name, 'TO MOCK MTM DEVICE: ', mtm_name)
self._mtm_handle = ProxyMTM(mtm_name)
self._msg_counter = 0
def set_base_frame(self, frame):
self.base_frame = frame
pass
def set_tip_frame(self, frame):
self.tip_frame = frame
pass
def pose_cb(self, msg):
cur_frame = msg_pose_to_kdl_frame(msg)
p = cur_frame.p
rpy = cur_frame.M.GetRPY()
# print (round(p[0], 3), round(p[1], 3), round(p[2], 3))
# print (round(rpy[0], 3), round(rpy[1], 3), round(rpy[2], 3))
# Convert the position based on the scale
cur_frame.p = cur_frame.p * self._scale
self.pose = self.base_frame.Inverse() * cur_frame * self.tip_frame
# Mark active as soon as first message comes through
self._active = True
pass
def buttons_cb(self, msg):
self.white_button_pressed = msg.white_button
self._geomagicButtonMsg = msg
# If any of the two pos clutch buttons are pressed. enable clutch
if self._geomagicButtonMsg.grey_button or self._externalFootPedalMsg.y == 1:
self.grey_button_pressed = True
else:
self.grey_button_pressed = False
def extra_footpedal_cb(self, msg):
if msg.x == 1.0:
self._engageMTM = True
elif msg.x == 0.0:
self._engageMTM = False
self._externalFootPedalMsg = msg
# If any of the two pos clutch buttons are pressed. enable clutch
if self._externalFootPedalMsg.y == 1 or self._geomagicButtonMsg.grey_button:
self.grey_button_pressed = True
else:
self.grey_button_pressed = False
def command_force(self, force):
pass
def process_commands(self):
if self._active:
if self._engageMTM:
self._mtm_handle.set_pose(self.pose)
self._mtm_handle.set_gripper_angle(self.white_button_pressed)
self._mtm_handle.set_foot_pedal(self.grey_button_pressed)
self._mtm_handle.publish_status()
force = self._mtm_handle.get_commanded_force()
force = self.base_frame.M.Inverse() * force
self._force.force.x = force[0]
self._force.force.y = force[1]
self._force.force.z = force[2]
if self._msg_counter % 500 == 0:
# print (self._force)
pass
if self._msg_counter >= 1000:
self._msg_counter = 0
# self._force_pub.publish(self._force)
self._msg_counter = self._msg_counter + 1