class CartVel:
def __init__(self):
self.pub = rospy.Publisher("/movo/right_arm/cartesian_vel_cmd", \
JacoCartesianVelocityCmd, queue_size=1)
self.rate = rospy.Rate(50)
def sendCmd(self, (x, y, z), (roll, pitch, yaw), time):
cmd = JacoCartesianVelocityCmd()
cmd.header.stamp = rospy.Time.now()
cmd.x = x
cmd.y = y
cmd.z = z
cmd.theta_x = roll
cmd.theta_y = pitch
cmd.theta_z = yaw
end_time = rospy.Time.now() + rospy.Duration(time)
rospy.loginfo("sending cartesian vel command: %s " % cmd)
while not rospy.is_shutdown() and rospy.Time.now() < end_time:
self.pub.publish(cmd)
self.rate.sleep()
rospy.loginfo("Done!")
def create_jaco_pose_vel_cmd(self, cmd_data):
pose_cmd = JacoCartesianVelocityCmd()
pose_cmd.x = cmd_data[0]
pose_cmd.y = cmd_data[1]
pose_cmd.z = cmd_data[2]
pose_cmd.theta_x = cmd_data[3]
pose_cmd.theta_y = cmd_data[4]
pose_cmd.theta_z = cmd_data[5]
return pose_cmd
def subscribe_force_callback_left(data):
global lf
lf = JacoCartesianVelocityCmd()
lf.x = data.wrench.force.x
lf.y = data.wrench.force.y
lf.z = data.wrench.force.z
lf.theta_x = data.wrench.torque.x
lf.theta_y = data.wrench.torque.y
lf.theta_z = data.wrench.torque.z
def subscribe_force_callback_right(data):
# print("========")
# print(data)
global rf
rf = JacoCartesianVelocityCmd()
rf.x = data.wrench.force.x
rf.y = data.wrench.force.y
rf.z = data.wrench.force.z
rf.theta_x = data.wrench.torque.x
rf.theta_y = data.wrench.torque.y
rf.theta_z = data.wrench.torque.z
def move(self, arm, le_ri=0, up_dn=0):
raise NotImplemented("try move_relate")
'''
le_ri: [-1, 1]
up_dn: [-1, 1]
'''
assert arm in ['left', 'right']
vel_x = 0.15 * le_ri
vel_y = -0.15 * up_dn
vel_z = self.insert_speed
arm_cmd = JacoCartesianVelocityCmd()
arm_cmd.header.stamp = rospy.get_rostime()
arm_cmd.header.frame_id = ''
arm_cmd.x = vel_x
arm_cmd.y = vel_y
arm_cmd.z = vel_z
r = rospy.Rate(1000)
if arm == 'left':
timeout = time.time() + 0.05
while not rospy.is_shutdown():
self.arm_pub[0].publish(arm_cmd)
if time.time() > timeout:
break
r.sleep()
print('run')
if arm == 'right':
timeout = time.time() + 0.05
while not rospy.is_shutdown():
self.arm_pub[1].publish(arm_cmd)
if time.time() > timeout:
break
r.sleep()
print('run')
def _movo_teleop(self, joyMessage):
self._parse_joy_input(joyMessage)
dt = rospy.get_time() - self.last_joy
self.last_joy = rospy.get_time()
if self.button_state['MAP_BASE_A']:
self.teleop_control_mode_msg.data = "base_ctl"
self.run_arm_ctl_right = False
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
elif self.button_state['MAP_rARM_B']:
self.teleop_control_mode_msg.data = "arm_ctl_right"
self.run_arm_ctl_right = True
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
elif self.button_state['MAP_lARM_X']:
self.teleop_control_mode_msg.data = "arm_ctl_left"
self.run_arm_ctl_right = False
self.run_arm_ctl_left = True
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
elif self.button_state['MAP_HEAD_Y']:
self.teleop_control_mode_msg.data = "pan_tilt_ctl"
# only mux topic when run_pan_tilt_ctl is changed from False to true
if self.run_pan_tilt_ctl == False:
self.head_cmd_current_source_index = (
self.head_cmd_current_source_index + 1) % len(
self.head_cmd_source)
rospy.wait_for_service('/head_cmd_mux/select')
switch_request = MuxSelectRequest()
switch_request.topic = '/movo/head/' + self.head_cmd_source[
self.head_cmd_current_source_index] + '/cmd'
self.head_cmd_srv.call(switch_request)
self.run_arm_ctl = False
self.run_arm_ctl_right = False
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = True
self._init_pan_tilt = True
if self.button_state['MAP_eSTOP_power']:
self.teleop_control_mode_msg.data = "estop"
self.run_arm_ctl = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
arm_cmd = JacoCartesianVelocityCmd()
arm_cmd.header.stamp = rospy.get_rostime()
arm_cmd.header.frame_id = ''
self.arm_pub[0].publish(arm_cmd)
self.arm_pub[1].publish(arm_cmd)
if self.button_state['MAP_HOME_start']:
self.teleop_control_mode_msg.data = "home_arms"
if not (self.homing_sent):
tmp = Bool()
tmp.data = True
self.home_arm_pub.publish(tmp)
self.homing_sent = True
self.homing_start_time = rospy.get_time()
if (self.homing_sent):
if ((rospy.get_time() - self.homing_start_time) >
10.0) and not self.button_state['MAP_HOME_start']:
self.homing_sent = False
if self.button_state['MAP_eSTOP_power']:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = DISABLE_REQUEST
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
"""
publish current control mode to inform other nodes
"""
self.teleop_control_mode_pub.publish(self.teleop_control_mode_msg)
if self.run_arm_ctl_right or self.run_arm_ctl_left:
arm_cmd = JacoCartesianVelocityCmd()
arm_cmd.header.stamp = rospy.get_rostime()
arm_cmd.header.frame_id = ''
gripper_cmd = GripperCmd()
kg_cmd = Float32()
if self.run_arm_ctl_right:
arm_idx = 0
else:
arm_idx = 1
if self.axis_value['MAP_TRIGGER_RT'] == -1.0:
if 0 == arm_idx:
arm_cmd.x = self.axis_value[
'MAP_TWIST_LR_stickleft'] * -self.arm_vel_lim
else:
arm_cmd.x = self.axis_value[
'MAP_TWIST_LR_stickleft'] * -self.arm_vel_lim
arm_cmd.z = self.axis_value[
'MAP_TWIST_UD_stickleft'] * self.arm_vel_lim
if not self.button_state['MAP_TORS_back_']:
arm_cmd.y = self.axis_value[
'MAP_TWIST_UD_stickright'] * -self.arm_vel_lim
else:
dt = rospy.get_time() - self.last_arm_update
self.lincmd.desired_position_m += (
self.axis_value['MAP_TWIST_UD_stickright'] *
self.lin_act_vel_lim) * dt
if (self.lincmd.desired_position_m > 0.464312):
self.lincmd.desired_position_m = 0.464312
elif self.lincmd.desired_position_m < 0.0:
self.lincmd.desired_position_m = 0.0
self.lincmd.header.stamp = rospy.get_rostime()
self.lincmd.header.frame_id = ''
self.linpub.publish(self.lincmd)
self.lincmd.header.seq += 1
self.last_arm_update = rospy.get_time()
arm_cmd.theta_y = self.axis_value['MAP_CROSS_LR'] * 100.0
arm_cmd.theta_x = self.axis_value['MAP_CROSS_UD'] * 100.0
if self.button_state['MAP_lWRIST_LB']:
arm_cmd.theta_z = 100.0
elif self.button_state['MAP_rWRIST_RB']:
arm_cmd.theta_z = -100.0
if self.button_state['MAP_stby_rstick']:
kg_cmd.data = 5000.0
elif self.button_state['MAP_TRACT_lstick']:
kg_cmd.data = -5000.0
else:
kg_cmd.data = 0.0
gripper_val = (self.axis_value['MAP_TRIGGER_LT'] + 1.0) / 2.0
if abs(self._last_gripper_val - gripper_val) > 0.05:
gripper_cmd.position = gripper_val * 0.085
gripper_cmd.speed = 0.05
gripper_cmd.force = 100.0
self.gripper_pub[arm_idx].publish(gripper_cmd)
self._last_gripper_val = gripper_val
self.arm_pub[arm_idx].publish(arm_cmd)
self.kgripper_pub[arm_idx].publish(kg_cmd)
elif self.run_pan_tilt_ctl:
if self.head_cmd_source[
self.head_cmd_current_source_index] == 'teleop':
vel_cmd = [0.0, 0.0]
if self.axis_value['MAP_TRIGGER_RT'] == -1.0:
vel_cmd[0] = -self.axis_value[
'MAP_TWIST_LR_stickleft'] * self.pt_vel_lim
vel_cmd[1] = -self.axis_value[
'MAP_TWIST_UD_stickright'] * self.pt_vel_lim
self.pt_cmds.pan_cmd.pos_rad += vel_cmd[0] * dt
self.pt_cmds.tilt_cmd.pos_rad += vel_cmd[1] * dt
self.pt_cmds.pan_cmd.pos_rad = limit_f(
self.pt_cmds.pan_cmd.pos_rad, (math.pi / 2.0))
self.pt_cmds.tilt_cmd.pos_rad = limit_f(
self.pt_cmds.tilt_cmd.pos_rad, (math.pi / 2.0))
self.pt_cmds.pan_cmd.vel_rps = 50.0 * (math.pi / 180.0)
self.pt_cmds.tilt_cmd.vel_rps = 50.0 * (math.pi / 180.0)
self.p_pub.publish(self.pt_cmds)
else:
if self.button_state['MAP_eSTOP_power']:
#handled at top of function
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_NONE'
self.cfg_cmd.gp_param = 0
elif self.button_state['MAP_stby_rstick']:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = STANDBY_REQUEST
elif self.button_state['MAP_TRACT_lstick']:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = TRACTOR_REQUEST
else:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_NONE'
self.cfg_cmd.gp_param = 0
if ('GENERAL_PURPOSE_CMD_NONE' != self.cfg_cmd.gp_cmd):
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
self.send_cmd_none = True
elif (True == self.send_cmd_none):
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
self.send_cmd_none = False
elif (False == self.send_cmd_none):
if self.axis_value['MAP_TRIGGER_RT'] == -1.0:
self.motion_cmd.linear.x = (
self.axis_value['MAP_TWIST_UD_stickleft'] *
self.x_vel_limit_mps)
self.motion_cmd.linear.y = (
self.axis_value['MAP_TWIST_LR_stickleft'] *
self.y_vel_limit_mps)
self.motion_cmd.angular.z = (
self.axis_value['MAP_TWIST_LR_stickright'] *
self.yaw_rate_limit_rps)
self.last_motion_command_time = rospy.get_time()
else:
self.motion_cmd.linear.x = 0.0
self.motion_cmd.linear.y = 0.0
self.motion_cmd.angular.z = 0.0
if (dt >= 0.01):
self.limited_cmd.linear.x = slew_limit(
self.motion_cmd.linear.x, self.limited_cmd.linear.x,
self.accel_lim, dt)
self.limited_cmd.linear.y = slew_limit(
self.motion_cmd.linear.y, self.limited_cmd.linear.y,
self.accel_lim, dt)
self.limited_cmd.angular.z = slew_limit(
self.motion_cmd.angular.z, self.limited_cmd.angular.z,
self.yaw_accel_lim, dt)
if ((rospy.get_time() - self.last_motion_command_time) <
2.0):
self.motion_pub.publish(self.limited_cmd)
def _movo_teleop(self, joyMessage):
self._parse_joy_input(joyMessage)
dt = rospy.get_time() - self.last_joy
self.last_joy = rospy.get_time()
if self.button_state['base_ctl']:
self.run_arm_ctl_right = False
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
elif self.button_state['arm_ctl_right']:
self.run_arm_ctl_right = True
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
elif self.button_state['arm_ctl_left']:
self.run_arm_ctl_right = False
self.run_arm_ctl_left = True
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
elif self.button_state['pan_tilt_ctl']:
self.run_arm_ctl = False
self.run_arm_ctl_right = False
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = True
self._init_pan_tilt = True
if self.button_state['estop']:
self.run_arm_ctl = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
arm_cmd = JacoCartesianVelocityCmd()
arm_cmd.header.stamp = rospy.get_rostime()
arm_cmd.header.frame_id = ''
self.arm_pub[0].publish(arm_cmd)
self.arm_pub[1].publish(arm_cmd)
if self.button_state['home_arms']:
if not (self.homing_sent):
tmp = Bool()
tmp.data = True
self.home_arm_pub.publish(tmp)
self.homing_sent = True
self.homing_start_time = rospy.get_time()
if (self.homing_sent):
if ((rospy.get_time() - self.homing_start_time) >
10.0) and not self.button_state['home_arms']:
self.homing_sent = False
if self.button_state['estop']:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = DISABLE_REQUEST
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
if self.run_arm_ctl_right or self.run_arm_ctl_left:
arm_cmd = JacoCartesianVelocityCmd()
arm_cmd.header.stamp = rospy.get_rostime()
arm_cmd.header.frame_id = ''
gripper_cmd = GripperCmd()
kg_cmd = Float32()
if self.run_arm_ctl_right:
arm_idx = 0
else:
arm_idx = 1
if self.button_state['dead_man']:
if 0 == arm_idx:
arm_cmd.x = self.axis_value[
'left_right'] * -self.arm_vel_lim
else:
arm_cmd.x = self.axis_value['left_right'] * self.arm_vel_lim
arm_cmd.z = self.axis_value['for_aft'] * self.arm_vel_lim
if not self.button_state['man_ovvrd']:
arm_cmd.y = self.axis_value['twist'] * self.arm_vel_lim
else:
dt = rospy.get_time() - self.last_arm_update
self.lincmd.desired_position_m += (
self.axis_value['twist'] * self.lin_act_vel_lim) * dt
if (self.lincmd.desired_position_m > 0.464312):
self.lincmd.desired_position_m = 0.464312
elif self.lincmd.desired_position_m < 0.0:
self.lincmd.desired_position_m = 0.0
self.lincmd.header.stamp = rospy.get_rostime()
self.lincmd.header.frame_id = ''
self.linpub.publish(self.lincmd)
self.lincmd.header.seq += 1
self.last_arm_update = rospy.get_time()
arm_cmd.theta_y = self.axis_value['dpad_ud'] * 100.0
arm_cmd.theta_x = self.axis_value['dpad_lr'] * 100.0
if self.button_state['wrist1']:
arm_cmd.theta_z = 100.0
elif self.button_state['wrist2']:
arm_cmd.theta_z = -100.0
if self.button_state['standby']:
kg_cmd.data = 5000.0
elif self.button_state['tractor']:
kg_cmd.data = -5000.0
else:
kg_cmd.data = 0.0
gripper_val = (self.axis_value['flipper'] + 1.0) / 2.0
if abs(self._last_gripper_val - gripper_val) > 0.05:
gripper_cmd.position = gripper_val * 0.085
gripper_cmd.speed = 0.05
gripper_cmd.force = 100.0
self.gripper_pub[arm_idx].publish(gripper_cmd)
self._last_gripper_val = gripper_val
self.arm_pub[arm_idx].publish(arm_cmd)
self.kgripper_pub[arm_idx].publish(kg_cmd)
elif self.run_pan_tilt_ctl:
vel_cmd = [0.0, 0.0]
if self.button_state['dead_man']:
vel_cmd[0] = -self.axis_value['twist'] * self.pt_vel_lim
vel_cmd[1] = -self.axis_value['for_aft'] * self.pt_vel_lim
self.pt_cmds.pan_cmd.pos_rad += vel_cmd[0] * dt
self.pt_cmds.tilt_cmd.pos_rad += vel_cmd[1] * dt
self.pt_cmds.pan_cmd.pos_rad = limit_f(
self.pt_cmds.pan_cmd.pos_rad, (math.pi / 2.0))
self.pt_cmds.tilt_cmd.pos_rad = limit_f(
self.pt_cmds.tilt_cmd.pos_rad, (math.pi / 2.0))
self.pt_cmds.pan_cmd.vel_rps = 50.0 * (math.pi / 180.0)
self.pt_cmds.tilt_cmd.vel_rps = 50.0 * (math.pi / 180.0)
self.p_pub.publish(self.pt_cmds)
else:
if self.button_state['estop']:
#handled at top of function
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_NONE'
self.cfg_cmd.gp_param = 0
elif self.button_state['standby']:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = STANDBY_REQUEST
elif self.button_state['tractor']:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = TRACTOR_REQUEST
else:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_NONE'
self.cfg_cmd.gp_param = 0
if ('GENERAL_PURPOSE_CMD_NONE' != self.cfg_cmd.gp_cmd):
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
self.send_cmd_none = True
elif (True == self.send_cmd_none):
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
self.send_cmd_none = False
elif (False == self.send_cmd_none):
if self.button_state['dead_man']:
self.motion_cmd.linear.x = (self.axis_value['for_aft'] *
self.x_vel_limit_mps)
self.motion_cmd.linear.y = (self.axis_value['left_right'] *
self.y_vel_limit_mps)
self.motion_cmd.angular.z = (self.axis_value['twist'] *
self.yaw_rate_limit_rps)
self.last_motion_command_time = rospy.get_time()
else:
self.motion_cmd.linear.x = 0.0
self.motion_cmd.linear.y = 0.0
self.motion_cmd.angular.z = 0.0
if (dt >= 0.01):
self.limited_cmd.linear.x = slew_limit(
self.motion_cmd.linear.x, self.limited_cmd.linear.x,
self.accel_lim, dt)
self.limited_cmd.linear.y = slew_limit(
self.motion_cmd.linear.y, self.limited_cmd.linear.y,
self.accel_lim, dt)
self.limited_cmd.angular.z = slew_limit(
self.motion_cmd.angular.z, self.limited_cmd.angular.z,
self.yaw_accel_lim, dt)
if ((rospy.get_time() - self.last_motion_command_time) <
2.0):
self.motion_pub.publish(self.limited_cmd)
if self.button_state[
'man_ovvrd'] and self.button_state['man_ovvrd']:
self.override_pub.publish(self.motion_cmd)
def __init__(self,
prefix="",
gripper="",
interface='eth0',
jaco_ip="10.66.171.15",
dof=""):
"""
Constructor
"""
# Setup a lock for accessing data in the control loop
self._lock = threading.Lock()
# Assume success until posted otherwise
rospy.loginfo('Starting JACO2 control')
self.init_success = True
self._prefix = prefix
self.iface = interface
self.arm_dof = dof
self.gripper = gripper
# List of joint names
if ("6dof" == self.arm_dof):
self._joint_names = [
self._prefix + '_shoulder_pan_joint',
self._prefix + '_shoulder_lift_joint',
self._prefix + '_elbow_joint', self._prefix + '_wrist_1_joint',
self._prefix + '_wrist_2_joint',
self._prefix + '_wrist_3_joint'
]
elif ("7dof" == self.arm_dof):
self._joint_names = [
self._prefix + '_shoulder_pan_joint',
self._prefix + '_shoulder_lift_joint',
self._prefix + '_arm_half_joint',
self._prefix + '_elbow_joint',
self._prefix + '_wrist_spherical_1_joint',
self._prefix + '_wrist_spherical_2_joint',
self._prefix + '_wrist_3_joint'
]
else:
rospy.logerr(
"DoF needs to be set 6 or 7, cannot start SIArmController")
return
self._num_joints = len(self._joint_names)
# Create the hooks for the API
if ('left' == prefix):
self.api = KinovaAPI('left', self.iface, jaco_ip, '255.255.255.0',
24000, 24024, 44000, self.arm_dof)
elif ('right' == prefix):
self.api = KinovaAPI('right', self.iface, jaco_ip, '255.255.255.0',
25000, 25025, 55000, self.arm_dof)
else:
rospy.logerr(
"prefix needs to be set to left or right, cannot start the controller"
)
return
if not (self.api.init_success):
self.Stop()
return
self.api.SetCartesianControl()
self._position_hold = False
self.estop = False
# Initialize the joint feedback
pos = self.api.get_angular_position()
vel = self.api.get_angular_velocity()
force = self.api.get_angular_force()
self._joint_fb = dict()
self._joint_fb['position'] = pos[:self._num_joints]
self._joint_fb['velocity'] = vel[:self._num_joints]
self._joint_fb['force'] = force[:self._num_joints]
if ("kg2" == gripper) or ("rq85" == gripper):
self._gripper_joint_names = [
self._prefix + '_gripper_finger1_joint',
self._prefix + '_gripper_finger2_joint'
]
self.num_fingers = 2
elif ("kg3" == gripper):
self._gripper_joint_names = [
self._prefix + '_gripper_finger1_joint',
self._prefix + '_gripper_finger2_joint',
self._prefix + '_gripper_finger3_joint'
]
self.num_fingers = 3
if (0 != self.num_fingers):
self._gripper_fb = dict()
self._gripper_fb['position'] = pos[self.
_num_joints:self._num_joints +
self.num_fingers]
self._gripper_fb['velocity'] = vel[self.
_num_joints:self._num_joints +
self.num_fingers]
self._gripper_fb['force'] = force[self.
_num_joints:self._num_joints +
self.num_fingers]
"""
Reset gravity vector to [0.0 9.81 0.0], along with positive y axis of kinova_arm base
"""
self.api.set_gravity_vector(0.0, 9.81, 0.0)
"""
Register the publishers and subscribers
"""
self.last_cartesian_vel_cmd_update = rospy.get_time() - 0.5
# X, Y, Z, ThetaX, ThetaY, ThetaZ, FingerVel
self._last_cartesian_vel_cmd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self._cartesian_vel_cmd_sub = rospy.Subscriber(
"/movo/%s_arm/cartesian_vel_cmd" % self._prefix,
JacoCartesianVelocityCmd, self._update_cartesian_vel_cmd)
self.last_angular_vel_cmd_update = rospy.get_time() - 0.5
self._last_angular_vel_cmd = [0.0
] * (self._num_joints + self.num_fingers)
if "6dof" == self.arm_dof:
self._angular_vel_cmd_sub = rospy.Subscriber(
"/movo/%s_arm/angular_vel_cmd" % self._prefix,
JacoAngularVelocityCmd6DOF, self._update_angular_vel_cmd)
elif "7dof" == self.arm_dof:
self._angular_vel_cmd_sub = rospy.Subscriber(
"/movo/%s_arm/angular_vel_cmd" % self._prefix,
JacoAngularVelocityCmd7DOF, self._update_angular_vel_cmd)
else:
# Error condition
rospy.logerr("DoF needs to be set 6 or 7, was {}".format(
self.arm_dof))
self.Stop()
return
self._gripper_vel_cmd = 0.0
self._ctl_mode = SIArmController.TRAJECTORY
self.api.set_control_mode(KinovaAPI.ANGULAR_CONTROL)
self._jstpub = rospy.Publisher("/movo/%s_arm_controller/state" %
self._prefix,
JointTrajectoryControllerState,
queue_size=10)
self._jstmsg = JointTrajectoryControllerState()
self._jstmsg.header.seq = 0
self._jstmsg.header.frame_id = ''
self._jstmsg.header.stamp = rospy.get_rostime()
self._jspub = rospy.Publisher("/movo/%s_arm/joint_states" %
self._prefix,
JointState,
queue_size=10)
self._jsmsg = JointState()
self._jsmsg.header.seq = 0
self._jsmsg.header.frame_id = ''
self._jsmsg.header.stamp = rospy.get_rostime()
self._jsmsg.name = self._joint_names
self._actfdbk_pub = rospy.Publisher("/movo/%s_arm/actuator_feedback" %
self._prefix,
KinovaActuatorFdbk,
queue_size=10)
self._actfdbk_msg = KinovaActuatorFdbk()
self._jsmsg.header.seq = 0
self._jsmsg.header.frame_id = ''
self._jsmsg.header.stamp = rospy.get_rostime()
if (0 != self.num_fingers):
self._gripper_vel_cmd_sub = rospy.Subscriber(
"/movo/%s_gripper/vel_cmd" % self._prefix, Float32,
self._update_gripper_vel_cmd)
if ("rq85" != gripper):
self._gripper_jspub = rospy.Publisher(
"/movo/%s_gripper/joint_states" % self._prefix,
JointState,
queue_size=10)
self._gripper_jsmsg = JointState()
self._gripper_jsmsg.header.seq = 0
self._gripper_jsmsg.header.frame_id = ''
self._gripper_jsmsg.header.stamp = rospy.get_rostime()
self._gripper_jsmsg.name = self._gripper_joint_names
self._cartesianforce_pub = rospy.Publisher(
"/movo/%s_arm/cartesianforce" % self._prefix,
JacoCartesianVelocityCmd,
queue_size=10)
self._cartesianforce_msg = JacoCartesianVelocityCmd()
self._cartesianforce_msg.header.seq = 0
self._cartesianforce_msg.header.frame_id = ''
self._cartesianforce_msg.header.stamp = rospy.get_rostime()
self._angularforce_gravityfree_pub = rospy.Publisher(
"/movo/%s_arm/angularforce_gravityfree" % self._prefix,
JacoStatus,
queue_size=10)
self._angularforce_gravityfree_msg = JacoStatus()
self._angularforce_gravityfree_msg.header.seq = 0
self._angularforce_gravityfree_msg.header.frame_id = ''
self._angularforce_gravityfree_msg.header.stamp = rospy.get_rostime()
self._angularforce_gravityfree_msg.type = "angularforce_gravityfree"
"""
This starts the controller in cart vel mode so that teleop is active by default
"""
if (0 != self.num_fingers):
self._gripper_pid = [None] * self.num_fingers
for i in range(self.num_fingers):
self._gripper_pid[i] = JacoPID(5.0, 0.0, 0.8)
self._gripper_vff = DifferentiateSignals(
self.num_fingers, self._gripper_fb['position'])
self._gripper_rate_limit = RateLimitSignals(
[FINGER_ANGULAR_VEL_LIMIT] * self.num_fingers,
self.num_fingers, self._gripper_fb['position'])
if ("6dof" == self.arm_dof):
self._arm_rate_limit = RateLimitSignals(JOINT_6DOF_VEL_LIMITS,
self._num_joints,
self._joint_fb['position'])
if ("7dof" == self.arm_dof):
self._arm_rate_limit = RateLimitSignals(JOINT_7DOF_VEL_LIMITS,
self._num_joints,
self._joint_fb['position'])
self._arm_vff_diff = DifferentiateSignals(self._num_joints,
self._joint_fb['position'])
self._pid = [None] * self._num_joints
for i in range(self._num_joints):
self._pid[i] = JacoPID(5.0, 0.0, 0.8)
self.pause_controller = False
self._init_ext_joint_position_control()
self._init_ext_gripper_control()
# Update the feedback once to get things initialized
self._update_controller_data()
# Start the controller
rospy.loginfo("Starting the %s controller" % self._prefix)
self._done = False
self._t1 = rospy.Timer(rospy.Duration(0.01), self._run_ctl)
def __init__(self):
dof = rospy.get_param('/init_robot/jaco_dof', '6dof')
if dof == '6dof':
self._dof = 6
# below which considered as a level noise could reach, no external force applied to corresponding joint except gravity force.
# values are based on test data when robot in DIFFERENT configurations, does not guarantee no external is applied for sure.
# can be used as a "soft" "loose" "lower" boundary to detect external joint force
self.angular_force_gravity_free_deadzone = [
1.5, 1.5, 1.5, 0.5, 2.0, 0.5
]
elif dof == '7dof':
self._dof = 7
# TODO: find thresholds for 7DOF based on experiments
# below which considered as a level noise could reach, no external force applied to corresponding joint except gravity force.
# values are based on test data when robot in different configuration, does not guarantee no external is applied for sure.
# can be used as a "soft" "loose" "lower" boundary to detect external joint force
self.angular_force_gravity_free_deadzone = [10.0] * self._dof
else:
raise ValueError(
"Please check ros parameter /init_robot/jaco_dof, it should be either '6dof' or '7dof' "
)
# define the interpolation shape between force and speed of movo base
# IMPORTANT: cartesian force range and offset are defined in the PRE-DEFINED follow-me-hand-pick-pose. They are not general, and could vary in large range for other arm configurations. If you define another follow-me-hand-pick-pose, make sure tune these values accordingly. eg: in the predefined pose, force_x is around +2.5, max applied force is 20, then range can be defined as [x+2.5 for x in [-20, 20].
self._base_cartesian_force_x_range = [
x + 2.5 for x in [-20.0, -13.0, -7.0, 7.0, 13.0, 20.0]
]
self._base_translation_speed_x_range = [-0.5, -0.3, 0.0, 0.0, 0.3, 0.5]
self._base_cartesian_force_y_range = [
x - 0.5 for x in [-20.0, -13.0, -10.0, 10.0, 13.0, 20.0]
] # add offset without applied force
self._base_translation_speed_y_range = [-0.4, -0.3, 0.0, 0.0, 0.3, 0.4]
self._base_cartesian_torque_z_range = [
x + 0.05 for x in [-5.0, -3.5, -3.0, 3.0, 3.5, 5.0]
]
self._base_rotation_speed_z_range = [-0.8, -0.6, 0.0, 0.0, 0.6, 0.8]
self._base_rotation_torque_threshold = min(
map(abs, self._base_cartesian_torque_z_range))
# "translation" or "rotation"
self._base_motion_mode = ''
self._Tsampling = 0.01
self._sampling_time = rospy.get_rostime()
self._base_x_speed_filter = LowPassFilter(k=1.0,
Tconst=0.2,
Tsampling=0.01)
self._base_y_speed_filter = LowPassFilter(k=1.0,
Tconst=0.2,
Tsampling=0.01)
self._base_theta_z_speed_filter = LowPassFilter(k=1.0,
Tconst=0.1,
Tsampling=0.01)
self._is_first_run = True
self._start_time = rospy.get_rostime()
self._is_applied_force_valid = False
self._right_cartesian_force_sub = rospy.Subscriber(
"/movo/right_arm/cartesianforce", JacoCartesianVelocityCmd,
self._right_cartesian_force_cb)
# self._right_cartesian_force_sub = rospy.Subscriber("/movo/right_arm/cartesianforce")
self._base_force_msg = JacoCartesianVelocityCmd()
self._base_force_msg.header.seq = 0
self._base_force_msg.header.stamp = rospy.get_rostime()
self._base_force_msg.header.frame_id = "base_link"
self._teleop_control_mode_sub = rospy.Subscriber(
"movo/teleop/control_mode", String, self._teleop_control_mode_cb)
# base_ctl, arm_ctl_right, arm_ctl_left, pan_tilt_ctl, estop, home_arms
self._teleop_control_mode = 'base_ctl'
self._teleop_control_mode_mutex = threading.RLock()
self._angular_force_gravity_free_sub = rospy.Subscriber(
"/movo/right_arm/angularforce_gravityfree", JacoStatus,
self._angular_force_gravity_free_cb)
self._angular_force_gravity_free_mutex = threading.Lock()
self._angular_force_gravity_free = [0.0] * self._dof
"""
# # the transformation from _tf_update adds evident delay (1~10 seconds) to the computation of wrench in movo_base frame. thus abandoned
# self._armbase_to_movobase_trans = numpy.zeros((3,1))
# self._armbase_to_movobase_rot = numpy.eye(3)
# self._armeef_to_movobase_trans = numpy.zeros((3,1))
# self._armeef_to_movobase_rot = numpy.eye(3)
# self._tf_listener = tf.TransformListener()
# self._tf_mutex = threading.Lock()
# # self._tf_listener_timer = rospy.Timer(0.01, self._tf_listener_timer_cb)
# self._tf_thread = threading.Thread(target=self._tf_update)
# self._tf_thread.start()
"""
# for develop and debug purpose
self._base_force_pub = rospy.Publisher("/movo/base/cartesianforce",
JacoCartesianVelocityCmd,
queue_size=1)
self._base_cmd_pub = rospy.Publisher("/movo/base/follow_me/cmd_vel",
Twist,
queue_size=1)
# self._base_cmd_pub = rospy.Publisher("/movo/base/follow_me/cmd_vel2", Twist, queue_size = 1)
self._base_cfg_pub = rospy.Publisher("/movo/gp_command",
ConfigCmd,
queue_size=10)
self._base_cfg_msg = ConfigCmd()
rospy.loginfo("Follow Me initialization finished")
rospy.spin()