def state_msg(self, ref_state, type_msg):
if type_msg == 'state':
st = JointTrajectoryControllerState()
elif type_msg == 'feedback':
st = FollowJointTrajectoryFeedback()
st.joint_names = [self.gripper_joint_name]
st.header = ref_state.header
st.desired.positions.extend(
[p * 2.0 for p in ref_state.desired.positions[:1]])
st.desired.velocities.extend(ref_state.desired.velocities[:1])
st.desired.accelerations.extend(ref_state.desired.accelerations[:1])
st.desired.effort.extend(ref_state.desired.effort[:1])
st.desired.time_from_start = ref_state.desired.time_from_start
st.actual.positions.extend(
[p * 2.0for p in ref_state.actual.positions[:1]])
st.actual.velocities.extend(ref_state.actual.velocities[:1])
st.actual.accelerations.extend(ref_state.actual.accelerations[:1])
st.actual.effort.extend(ref_state.actual.effort[:1])
st.actual.time_from_start = ref_state.actual.time_from_start
st.error.positions.extend(
[p * 2.0 for p in ref_state.error.positions[:1]])
st.error.velocities.extend(ref_state.error.velocities[: 1])
st.error.accelerations.extend(ref_state.error.accelerations[: 1])
st.error.effort.extend(ref_state.error.effort[: 1])
return st
def state_msg(self, ref_state, type_msg):
if type_msg == 'state':
st = JointTrajectoryControllerState()
elif type_msg == 'feedback':
st = FollowJointTrajectoryFeedback()
st.joint_names = [self.gripper_joint_name]
st.header = ref_state.header
st.desired.positions.extend(
[p * 2.0 for p in ref_state.desired.positions[:1]])
st.desired.velocities.extend(ref_state.desired.velocities[:1])
st.desired.accelerations.extend(ref_state.desired.accelerations[:1])
st.desired.effort.extend(ref_state.desired.effort[:1])
st.desired.time_from_start = ref_state.desired.time_from_start
st.actual.positions.extend(
[p * 2.0 for p in ref_state.actual.positions[:1]])
st.actual.velocities.extend(ref_state.actual.velocities[:1])
st.actual.accelerations.extend(ref_state.actual.accelerations[:1])
st.actual.effort.extend(ref_state.actual.effort[:1])
st.actual.time_from_start = ref_state.actual.time_from_start
st.error.positions.extend(
[p * 2.0 for p in ref_state.error.positions[:1]])
st.error.velocities.extend(ref_state.error.velocities[:1])
st.error.accelerations.extend(ref_state.error.accelerations[:1])
st.error.effort.extend(ref_state.error.effort[:1])
return st
def js_cb(self, js_msg):
"""
:type js_msg: JointState
"""
try:
torso_offset = js_msg.name.index(self.torso_joint_name)
except ValueError:
rospy.logerr("No {} in omnidrive/joint_states. Can't get position.".format(self.torso_joint_name))
return
state_msg = JointTrajectoryControllerState()
state_msg.joint_names = [self.torso_joint_name]
self.state_pub.publish(state_msg)
def default(self, ci='unused'):
js = JointTrajectoryControllerState()
js.header = self.get_ros_header()
# timestamp is not a joint
joints = dict(self.data)
del joints['timestamp']
js.joint_names = joints.keys()
js.actual.positions = joints.values()
js.actual.velocities = [0.0] * len(joints)
js.actual.accelerations = [0.0] * len(joints)
self.publish(js)
def test_JointTrajectoryControllerState():
'''
PUBLISHER METHODE: JointControllerStates
'''
pub_JointControllerStates = rospy.Publisher('state', JointTrajectoryControllerState, queue_size=10)
message = JointTrajectoryControllerState()
message.header.frame_id = "testID: 101"
message .joint_names = ["fl_caster_r_wheel_joint", "bl_caster_r_wheel_joint", "br_caster_r_wheel_joint", "fr_caster_r_wheel_joint", "fl_caster_rotation_joint", "bl_caster_rotation_joint", "br_caster_rotation_joint", "fr_caster_rotation_joint"]
message.actual.velocities = [4,4,4,4,4,4,4,4]
message.actual.positions = [4,4,4,4,4,4,4,4]
print "JointControllerStates: " + message.header.frame_id
pub_JointControllerStates.publish(message)
def __init__(self):
self.start_to_sub = False
rospy.Subscriber('/manipulator_controller/state',
JointTrajectoryControllerState, self.callback)
self.compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group_name = "manipulator"
self.group = moveit_commander.MoveGroupCommander(self.group_name)
rospy.loginfo('move_group name: manipulator')
self.current_joint_values = self.group.get_current_joint_values()
self.current_jacobian_matrix = np.zeros((6, 6))
self.current_jacobian_matrix_det = 0
self.current_end_effector_pose = PoseStamped()
self.current_state = JointTrajectoryControllerState()
self.start_time = rospy.get_time()
self.got_msg = False
self.current_cartisian_velocity = np.zeros((6, 1))
self.move_type = 1 #1 means desired, 2 means noise_added, 3 means controlled
self.rate_hz = 50.0
self.is_control = False
self.active_joints = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
self.move_group_link_names = [
'first_link', 'shoulder_link', 'upper_arm_link', 'forearm_link',
'wrist_1_link', 'wrist_2_link', 'wrist_3_link'
]
self.K = [0.4, 0.4, 1, 0.6, 0.6, 0.6]
def __init__(self):
# Client
self.arm_client = SimpleActionClient(ARM_CLIENT_TOPIC,
FollowJointTrajectoryAction)
rospy.loginfo("ARM CLIENT: Waiting for arm action server...")
arm_client_running = self.arm_client.wait_for_server(rospy.Duration(2))
if arm_client_running:
rospy.loginfo("ARM CLIENT: Arm controller initialized.")
else:
rospy.loginfo("ARM CLIENT: Arm controller is NOT initialized!")
# Subscribe for states
rospy.Subscriber(ARM_STATE_TOPIC, JointTrajectoryControllerState,
self._update_state)
self.curr_arm_state_ = JointTrajectoryControllerState()
# Setup
self.arm_goal = FollowJointTrajectoryGoal()
self.arm_goal.trajectory.joint_names = ARM_JOINTS
self.p = JointTrajectoryPoint()
self.timeout = rospy.Duration(30)
# Pre-def. positions
r = RosPack()
self.yaml_filename = r.get_path(
'frasier_utilities') + '/config/arm_configs.yaml'
with open(self.yaml_filename, 'r') as stream:
try:
self.positions = yaml.load(stream)
except yaml.YAMLError as exc:
rospy.logwarn(
"ARM CLIENT: Yaml Exception Caught: {}".format(exc))
rospy.logdebug("ARM CLIENT: Config: {}".format(self.positions))
def __init__(self, move_type, write_type):
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group_name = "manipulator"
self.group = moveit_commander.MoveGroupCommander(self.group_name)
#rospy.loginfo('move_group name: manipulator')
self.compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
self.current_joint_values = self.group.get_current_joint_values()
self.current_cartisian_velocity = numpy.zeros((6, 1))
self.current_joint_velocity = []
self.current_jacobian_matrix = numpy.zeros((6, 6))
self.current_end_effector_pose = PoseStamped()
self.manipulator_state = JointTrajectoryControllerState()
self.start_time = rospy.get_time()
self.got_msg = False
self.active_joints = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
self.move_group_link_names = [
'first_link', 'shoulder_link', 'upper_arm_link', 'forearm_link',
'wrist_1_link', 'wrist_2_link', 'wrist_3_link'
]
self.move_type = move_type
self.write_type = write_type
rospy.Subscriber('manipulator_controller/state',
JointTrajectoryControllerState,
self.callback_sub_robot_state)
def joint_callback(self, data):
front_msg = JointTrajectoryControllerState()
back_msg = JointTrajectoryControllerState()
front_msg.joint_names.append('fl_caster_r_wheel_joint')
front_msg.joint_names.append('fr_caster_r_wheel_joint')
front_msg.desired.velocities.append(data.desired.velocities[0])
front_msg.desired.velocities.append(data.desired.velocities[6])
self.front_pub.publish(front_msg)
back_msg.joint_names.append('bl_caster_r_wheel_joint')
back_msg.joint_names.append('br_caster_r_wheel_joint')
back_msg.desired.velocities.append(data.desired.velocities[2])
back_msg.desired.velocities.append(data.desired.velocities[4])
self.back_pub.publish(back_msg)
result1 = self.turn_dynamixel(1, data.desired.positions[1])
result2 = self.turn_dynamixel(2, data.desired.positions[3])
result3 = self.turn_dynamixel(3, data.desired.positions[5])
result4 = self.turn_dynamixel(4, data.desired.positions[7])
def test_JointTrajectoryControllerState():
'''
PUBLISHER METHODE: JointControllerStates
'''
pub_JointControllerStates = rospy.Publisher('state',
JointTrajectoryControllerState,
queue_size=10)
message = JointTrajectoryControllerState()
message.header.frame_id = "testID: 101"
message.joint_names = [
"fl_caster_r_wheel_joint", "bl_caster_r_wheel_joint",
"br_caster_r_wheel_joint", "fr_caster_r_wheel_joint",
"fl_caster_rotation_joint", "bl_caster_rotation_joint",
"br_caster_rotation_joint", "fr_caster_rotation_joint"
]
message.actual.velocities = [4, 4, 4, 4, 4, 4, 4, 4]
message.actual.positions = [4, 4, 4, 4, 4, 4, 4, 4]
print "JointControllerStates: " + message.header.frame_id
pub_JointControllerStates.publish(message)
def js_cb(self, js):
self.current_pose = np.array([
js.position[self.x_index_js], js.position[self.y_index_js],
js.position[self.z_index_js]
])
try:
current_vel = np.array([
js.velocity[self.x_index_js], js.velocity[self.y_index_js],
js.velocity[self.z_index_js]
])
except IndexError as e:
rospy.logwarn('velocity entry in joint state is empty')
return
time = js.header.stamp.to_sec()
self.last_cmd = current_vel
if not self.done:
time_from_start = time - self.start_time
if time_from_start > 0:
goal_pose = self.pose_traj2(time_from_start)
cmd = make_cmd(self.current_pose, goal_pose, self.last_cmd,
self.goal_vel2, time_from_start)
if np.any(np.isnan(cmd)):
print('f**k')
else:
cmd = self.limit_vel(cmd)
cmd = kdl_to_np(
js_to_kdl(*self.current_pose).M.Inverse() *
make_twist(*cmd))
self.debug_vel.publish(np_to_msg(cmd))
# cmd = self.hack(cmd)
cmd_msg = np_to_msg(cmd)
self.cmd_vel_sub.publish(cmd_msg)
self.last_cmd = cmd
state = JointTrajectoryControllerState()
state.joint_names = [x_joint, y_joint, z_joint]
self.state_pub.publish(state)
def __init__(self):
rospy.init_node("real_joint_state_publisher")
rospy.Subscriber("/chatter", String, self.encoder_callback)
self.pub = rospy.Publisher("/rover_arm_controller/command",
JointTrajectoryControllerState,
queue_size=50)
self.joint_states = JointTrajectoryControllerState()
self.joint_states.joint_names = [
"joint1", "joint2", "joint3", "joint4", "joint5", "joint6"
]
#self.joint_states.position = [1, 1, 1, 1, 1, 1]
#self.joint_states.effort = [10, 10, 10, 10, 10, 10]
self.rate = rospy.Rate(30)
while not rospy.is_shutdown():
self.state_publisher()
print(self.joint_states)
def __init__(self):
# The current status of the joints.
self.JointState = JointTrajectoryControllerState()
# The servo power's status of the robot.
self.ServoPowerState = Bool()
# The fault power's status of the robot.
self.PowerFaultState = Bool()
# The reference coordinate in the calculations of the elfin_basic_api node
self.RefCoordinate = String()
# The end coordinate in the calculations of the elfin_basic_api node
self.EndCoordinate = String()
#The value of the dynamic parameters of elfin_basic_api, e.g. velocity scaling.
self.DynamicArgs = Config()
# get the reference coordinate name of elfin_basic_api from the response of this service.
self.call_ref_coordinate = rospy.ServiceProxy(
'elfin_basic_api/get_reference_link', SetBool)
self.call_ref_coordinate_req = SetBoolRequest()
# get the end coordinate name of elfin_basic_api from the response of this service.
self.call_end_coordinate = rospy.ServiceProxy(
'elfin_basic_api/get_end_link', SetBool)
self.call_end_coordinate_req = SetBoolRequest()
# for publishing joint goals to elfin_basic_api
self.JointsPub = rospy.Publisher('elfin_basic_api/joint_goal',
JointState,
queue_size=1)
self.JointsGoal = JointState()
# for publishing cart goals to elfin_basic_api
self.CartGoalPub = rospy.Publisher('elfin_basic_api/cart_goal',
PoseStamped,
queue_size=1)
self.CartPos = PoseStamped()
# for pub cart path
self.CartPathPub = rospy.Publisher('elfin_basic_api/cart_path_goal',
PoseArray,
queue_size=1)
self.CartPath = PoseArray()
self.CartPath.header.stamp = rospy.get_rostime()
self.CartPath.header.frame_id = 'elfin_base_link'
# for pub one specific joint action to elfin_teleop_joint_cmd_no_limit
self.JointCmdPub = rospy.Publisher('elfin_teleop_joint_cmd_no_limit',
Int64,
queue_size=1)
self.JointCmd = Int64()
# action client, send goal to move_group
self.action_client = SimpleActionClient(
'elfin_module_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
self.action_goal = FollowJointTrajectoryGoal()
#self.goal_list = JointTrajectoryPoint()
self.goal_list = []
self.joints_ = []
self.ps_ = []
self.listener = tf.TransformListener()
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander('elfin_arm')
self.ref_link_name = self.group.get_planning_frame()
self.end_link_name = self.group.get_end_effector_link()
self.ref_link_lock = threading.Lock()
self.end_link_lock = threading.Lock()
self.call_teleop_stop = rospy.ServiceProxy(
'elfin_basic_api/stop_teleop', SetBool)
self.call_teleop_stop_req = SetBoolRequest()
self.call_teleop_joint = rospy.ServiceProxy(
'elfin_basic_api/joint_teleop', SetInt16)
self.call_teleop_joint_req = SetInt16Request()
self.call_teleop_cart = rospy.ServiceProxy(
'elfin_basic_api/cart_teleop', SetInt16)
self.call_teleop_cart_req = SetInt16Request()
self.call_move_homing = rospy.ServiceProxy(
'elfin_basic_api/home_teleop', SetBool)
self.call_move_homing_req = SetBoolRequest()
self.call_reset = rospy.ServiceProxy(
self.elfin_driver_ns + 'clear_fault', SetBool)
self.call_reset_req = SetBoolRequest()
self.call_reset_req.data = True
self.call_power_on = rospy.ServiceProxy(
self.elfin_driver_ns + 'enable_robot', SetBool)
self.call_power_on_req = SetBoolRequest()
self.call_power_on_req.data = True
self.call_power_off = rospy.ServiceProxy(
self.elfin_driver_ns + 'disable_robot', SetBool)
self.call_power_off_req = SetBoolRequest()
self.call_power_off_req.data = True
self.call_velocity_setting = rospy.ServiceProxy(
'elfin_basic_api/set_velocity_scale', SetFloat64)
self.call_velocity_req = SetFloat64Request()
self._velocity_scale = 0.78
self.set_velocity_scale(self._velocity_scale)
pass
#! /usr/bin/env python
import serial
import rospy
import numpy as np
import pigpio
import sys
from control_msgs.msg import JointTrajectoryControllerState
from geometry_msgs.msg import Vector3Stamped
from sensor_msgs.msg import JointState
from nav_msgs.msg import Odometry
pi = pigpio.pi()
msg = JointState()
mpc = JointTrajectoryControllerState()
class adq_datos(object):
def __init__(self):
self.pub = rospy.Publisher('/main_node', JointState, queue_size=10)
self.pub1 = rospy.Publisher('/MPC',
JointTrajectoryControllerState,
queue_size=10)
self.Ts = 0.05
self.modo = 0
self.calibration = 0
self.th = 0
self.st = 0
def js_cb(self, data):
msg = JointTrajectoryControllerState()
msg.joint_names = data.name
self.state_pub.publish(msg)
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):
rospy.init_node(NODE, anonymous=True)
print "initialized"
self.js=JointTrajectoryControllerState()
self.js_received=False
class BaseControl(object):
# create messages that are used to publish state/feedback/result
_state = JointTrajectoryControllerState()
_feedback = FollowJointTrajectoryActionFeedback()
_result = FollowJointTrajectoryActionResult()
def __init__(self):
self.cmd_vel_pub = rospy.Publisher('~cmd_vel', Twist, queue_size=10)
joint_states = rospy.wait_for_message('base/joint_states', JointState)
try:
self.odom_x_joint_index = joint_states.name.index(odom_x_joint)
self.odom_y_joint_index = joint_states.name.index(odom_y_joint)
self.odom_z_joint_index = joint_states.name.index(odom_z_joint)
rospy.loginfo("omni_pose_follower found odom joints")
except ValueError as e:
rospy.logwarn("omni_pose_follower couldn't find odom joints in joint states!")
return
# create tf listener
self.tf_listener = tf.TransformListener()
# set frames
self.set_odom_origin()
# create state publisher
self.state_pub = rospy.Publisher('{}/state'.format(name_space), JointTrajectoryControllerState, queue_size=10)
self.state_sub = rospy.Subscriber('base/joint_states', JointState, self.joint_states_callback, queue_size=10)
# create the action server
self._as = actionlib.SimpleActionServer('{}/follow_joint_trajectory'.format(name_space), FollowJointTrajectoryAction, self.goal_callback, False)
self._as.start()
def set_odom_origin(self):
try:
self.tf_listener.waitForTransform("map", odom, rospy.Time(), rospy.Duration(10))
except TransformException as e:
rospy.logwarn("omni_pose_follower couldn't find odom frame")
else:
t = self.tf_listener.getLatestCommonTime("map", odom)
pos, quat = self.tf_listener.lookupTransform("map", odom, t)
self.map_T_odom_origin = PyKDL.Frame(PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3]), PyKDL.Vector(pos[0], pos[1], pos[2]))
def joint_states_callback(self, joint_states):
try:
self.tf_listener.waitForTransform("map", base, rospy.Time(), rospy.Duration(1))
except TransformException as e:
rospy.logwarn("omni_pose_follower couldn't find map frame or base_footprint frame")
else:
t = self.tf_listener.getLatestCommonTime("map", base)
pos, quat = self.tf_listener.lookupTransform("map", base, t)
map_T_base_footprint = PyKDL.Frame(PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3]), PyKDL.Vector(pos[0], pos[1], pos[2]))
try:
odom_origin_T_map = self.map_T_odom_origin.Inverse()
except:
exit()
self.odom_origin_T_base_footprint = odom_origin_T_map * map_T_base_footprint
pos_x = self.odom_origin_T_base_footprint.p.x()
pos_y = self.odom_origin_T_base_footprint.p.y()
[_, _, euler_z] = self.odom_origin_T_base_footprint.M.GetRPY()
self._state.actual.positions = [pos_x, pos_y, euler_z]
try:
self._state.actual.velocities = [joint_states.velocity[self.odom_x_joint_index], joint_states.velocity[self.odom_y_joint_index], joint_states.velocity[self.odom_z_joint_index]]
except IndexError as e:
rospy.logwarn("omni_pose_follower couldn't find enough odom joint velocities in joint states!")
self._as.set_aborted()
return
# publish joint_states
self._state.header.stamp = rospy.Time.now()
self._state.header.frame_id = joint_states.header.frame_id
self._state.joint_names = joint_states.name
self.state_pub.publish(self._state)
def goal_callback(self, goal):
# helper variables
success = True
rate = rospy.Rate(freq)
try:
goal_odom_x_joint_index = goal.trajectory.joint_names.index(odom_x_joint)
goal_odom_y_joint_index = goal.trajectory.joint_names.index(odom_y_joint)
goal_odom_z_joint_index = goal.trajectory.joint_names.index(odom_z_joint)
except:
rospy.loginfo("omni_pose_follower aborted current goal")
self._as.set_aborted()
return
# check if the path is not too short
L = len(goal.trajectory.points)
s_x = sum([abs(goal.trajectory.points[t].velocities[goal_odom_x_joint_index]) for t in range(L)])
s_y = sum([abs(goal.trajectory.points[t].velocities[goal_odom_y_joint_index]) for t in range(L)])
s_z = sum([abs(goal.trajectory.points[t].velocities[goal_odom_z_joint_index]) for t in range(L)])
if s_x < S_x and s_y < S_y and s_z < S_z: # path is too short
rospy.loginfo("The goal has been reached")
self._result.result.error_string = "no error"
self._as.set_succeeded(self._result.result)
return
# initialization
cmd_vel = Twist()
t = 0
delta_T = 1/freq
time_start = goal.trajectory.header.stamp - rospy.Duration(T_delay)
self.set_odom_origin()
while True:
if self._as.is_preempt_requested():
rospy.loginfo("The goal has been preempted")
# the following line sets the client in preempted state (goal cancelled)
self._as.set_preempted()
success = False
break
time_from_start = rospy.Time.now() - time_start
# set goal velocites in map frame
if t < L and t >= 0:
for point_index in range(t,L):
if goal.trajectory.points[point_index].time_from_start < time_from_start:
t += 1
else:
break
if t == L:
t = -1
time_finish = rospy.Time.now()
if t == -1:
time_from_finish = rospy.Time.now() - time_finish
if time_from_finish.secs > T_finish:
success = True
break
v_x = goal.trajectory.points[t].velocities[goal_odom_x_joint_index]
v_y = goal.trajectory.points[t].velocities[goal_odom_y_joint_index]
v_z = goal.trajectory.points[t].velocities[goal_odom_z_joint_index]
pos = [goal.trajectory.points[t].positions[goal_odom_x_joint_index], goal.trajectory.points[t].positions[goal_odom_y_joint_index], 0]
quat = tf.transformations.quaternion_from_euler(0, 0, goal.trajectory.points[t].positions[goal_odom_z_joint_index])
odom_origin_T_base_footprint_goal = PyKDL.Frame(PyKDL.Rotation.Quaternion(quat[0], quat[1], quat[2], quat[3]), PyKDL.Vector(pos[0], pos[1], pos[2]))
base_footprint_T_odom_origin = self.odom_origin_T_base_footprint.Inverse()
base_footprint_T_base_footprint_goal = base_footprint_T_odom_origin * odom_origin_T_base_footprint_goal
error_odom_x_pos = base_footprint_T_base_footprint_goal.p.x()
error_odom_y_pos = base_footprint_T_base_footprint_goal.p.y()
[_,_,error_odom_z_pos] = base_footprint_T_base_footprint_goal.M.GetRPY()
# goal error
if t == -1:
if abs(error_odom_x_pos) + abs(error_odom_y_pos) + abs(error_odom_z_pos) < 0.001:
success = True
break
# print("At " + str(t) + ": error_traj = " + str([error_odom_x_pos, error_odom_y_pos, error_odom_z_pos]))
error_odom_x_vel = goal.trajectory.points[t].velocities[goal_odom_x_joint_index] - self._state.actual.velocities[0]
error_odom_y_vel = goal.trajectory.points[t].velocities[goal_odom_y_joint_index] - self._state.actual.velocities[1]
error_odom_z_vel = goal.trajectory.points[t].velocities[goal_odom_z_joint_index] - self._state.actual.velocities[2]
self._feedback.feedback.header.stamp = rospy.Time.now()
self._feedback.feedback.header.frame_id = self._state.header.frame_id
self._feedback.feedback.joint_names = self._state.joint_names
self._feedback.feedback.desired.positions = goal.trajectory.points[t].positions
self._feedback.feedback.desired.velocities = goal.trajectory.points[t].velocities
self._feedback.feedback.desired.time_from_start = time_from_start
self._feedback.feedback.actual.positions = self._state.actual.positions
self._feedback.feedback.actual.velocities = self._state.actual.velocities
self._feedback.feedback.actual.time_from_start = time_from_start
self._feedback.feedback.error.positions = [error_odom_x_pos, error_odom_y_pos, error_odom_z_pos]
self._feedback.feedback.error.velocities = [error_odom_x_vel, error_odom_y_vel, error_odom_z_vel]
self._feedback.feedback.error.time_from_start = time_from_start
# publish the feedback
self._as.publish_feedback(self._feedback.feedback)
# transform velocities from map frame to base frame and add feedback control
sin_z = math.sin(self._state.actual.positions[2])
cos_z = math.cos(self._state.actual.positions[2])
cmd_vel_x = v_x * cos_z + v_y * sin_z + K_x['p'] * error_odom_x_pos + K_x['d'] * error_odom_x_vel
cmd_vel_y = -v_x * sin_z + v_y * cos_z + K_y['p'] * error_odom_y_pos + K_y['d'] * error_odom_y_vel
cmd_vel_z = v_z + K_z['p'] * error_odom_z_pos + K_z['d'] * error_odom_z_vel
# add time delay
cmd_vel.linear.x = T_delay/(T_delay+delta_T) * cmd_vel.linear.x + delta_T/(T_delay+delta_T) * cmd_vel_x
cmd_vel.linear.y = T_delay/(T_delay+delta_T) * cmd_vel.linear.y + delta_T/(T_delay+delta_T) * cmd_vel_y
cmd_vel.angular.z = T_delay/(T_delay+delta_T) * cmd_vel.angular.z + delta_T/(T_delay+delta_T) * cmd_vel_z
# publish the velocity
self.cmd_vel_pub.publish(cmd_vel)
rate.sleep()
# set velocites to zero
self.cmd_vel_pub.publish(Twist())
if success:
rospy.loginfo("The goal has been reached, final diff: {}".format([error_odom_x_pos, error_odom_y_pos, error_odom_z_pos]))
self._result.result.error_string = "no error"
self._as.set_succeeded(self._result.result)
else:
self._as.set_aborted(self._result.result)
def step(self, action):
state = []
reward = []
done = False
data_arm_state = None
while data_arm_state is None:
try:
data_arm_state = rospy.wait_for_message('/ariac/arm/state', JointTrajectoryControllerState, timeout=5)
except:
pass
data_arm_state2 = JointTrajectoryControllerState()
data_arm_state2.joint_names = data_arm_state.joint_names[1:]
data_arm_state2.desired.positions = data_arm_state.desired.positions[1:]
data_arm_state2.desired.velocities = data_arm_state.desired.velocities[1:]
data_arm_state2.desired.accelerations = data_arm_state.desired.accelerations[1:]
data_arm_state2.actual.positions = data_arm_state.actual.positions[1:]
data_arm_state2.actual.velocities = data_arm_state.actual.velocities[1:]
data_arm_state2.actual.accelerations = data_arm_state.actual.accelerations[1:]
data_arm_state2.error.positions = data_arm_state.error.positions[1:]
data_arm_state2.error.velocities = data_arm_state.error.velocities[1:]
data_arm_state2.error.accelerations = data_arm_state.error.accelerations[1:]
# Collect the end effector points and velocities in
# cartesian coordinates for the process_observationsstate.
# Collect the present joint angles and velocities from ROS for the state.
last_observations = self.process_observations(data_arm_state2, self.environment)
# # # Get Jacobians from present joint angles and KDL trees
# # # The Jacobians consist of a 6x6 matrix getting its from from
# # # (# joint angles) x (len[x, y, z] + len[roll, pitch, yaw])
ee_link_jacobians = self.get_jacobians(last_observations)
if self.environment['link_names'][-1] is None:
print("End link is empty!!")
return None
else:
# print(self.environment['link_names'][-1])
trans, rot = forward_kinematics(self.scara_chain,
self.environment['link_names'],
last_observations[:self.scara_chain.getNrOfJoints()],
base_link=self.environment['link_names'][0],
end_link=self.environment['link_names'][-1])
# #
rotation_matrix = np.eye(4)
rotation_matrix[:3, :3] = rot
rotation_matrix[:3, 3] = trans
# angle, dir, _ = rotation_from_matrix(rotation_matrix)
# #
# current_quaternion = np.array([angle]+dir.tolist())#
# I need this calculations for the new reward function, need to send them back to the run scara or calculate them here
current_quaternion = quaternion_from_matrix(rotation_matrix)
current_ee_tgt = np.ndarray.flatten(get_ee_points(self.environment['end_effector_points'],
trans,
rot).T)
ee_points = current_ee_tgt - self.realgoal#self.environment['ee_points_tgt']
ee_points_jac_trans, _ = self.get_ee_points_jacobians(ee_link_jacobians,
self.environment['end_effector_points'],
rot)
ee_velocities = self.get_ee_points_velocities(ee_link_jacobians,
self.environment['end_effector_points'],
rot,
last_observations)
# Concatenate the information that defines the robot state
# vector, typically denoted asrobot_id 'x'.
state = np.r_[np.reshape(last_observations, -1),
np.reshape(ee_points, -1),
np.reshape(ee_velocities, -1),]
self._pub.publish(self.get_trajectory_message([0.018231524968342513,
3.2196073949389703 + np.random.rand(1)[0] - .5,
-0.6542426695478509 + np.random.rand(1)[0] - .5,
1.7401498071871018 + np.random.rand(1)[0] - .5,
3.7354939354077596 + np.random.rand(1)[0] - .5,
-1.510707301072792 + np.random.rand(1)[0] - .5,
0.00014565660628829136 + np.random.rand(1)[0] - .5]))
data_vacuum_state = None
while data_vacuum_state is None:
try:
data_vacuum_state = rospy.wait_for_message('/ariac/gripper/state', VacuumGripperState, timeout=5)
except:
pass
data_camera = None
while data_camera is None:
try:
data_camera = rospy.wait_for_message('/ariac/logical_camera_1', LogicalCameraImage, timeout=5)
except:
pass
try:
self.vacuum_gripper_control(enable=bool(state[0]))
except (rospy.ServiceException) as e:
print ("/gazebo/reset_simulation service call failed")
state = [data_arm_state, data_camera, data_vacuum_state]
return state, reward, done, {}
def __init__(self, movo_ip='10.66.171.5'):
self.init_success = False
"""
Create the thread to run MOVO Linear actuator command interface
"""
self._cmd_buffer = multiprocessing.Queue()
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((movo_ip, 6237),
self.tx_queue_,
self.rx_queue_,
max_packet_size=KINOVA_ACTUATOR_RSP_SIZE_BYTES)
if (False == self.comm.link_up):
rospy.logerr("Could not open socket for MOVO pan_tilt...exiting")
self.Shutdown()
return
"""
Initialize the publishers and subscribers for the node
"""
self.cmd_data = PanTiltCmd()
self._last_cmd = JointTrajectoryPoint()
self._init_cmds = True
self.s = rospy.Subscriber("/movo/head/cmd", PanTiltCmd,
self._add_motion_command_to_queue)
self._jcc = rospy.Subscriber("/movo/head_controller/command",
JointTrajectory,
self._add_traj_command_to_queue)
self.actuator_data = PanTiltFdbk()
self.actuator_pub = rospy.Publisher("/movo/head/data",
PanTiltFdbk,
queue_size=10)
self.js = JointState()
self.js_pub = rospy.Publisher("/movo/head/joint_states",
JointState,
queue_size=10)
self._jcs = JointTrajectoryControllerState()
self._jcs_pub = rospy.Publisher("/movo/head_controller/state",
JointTrajectoryControllerState,
queue_size=10)
self._jc_srv = rospy.Service('/movo/head_controller/query_state',
QueryTrajectoryState,
self._handle_state_query)
"""
Start the receive handler thread
"""
self.need_to_terminate = False
self.terminate_mutex = threading.RLock()
self.last_rsp_rcvd = rospy.get_time()
self._rcv_thread = threading.Thread(target=self._run)
self._rcv_thread.start()
self._t1 = rospy.Timer(rospy.Duration(0.01),
self._update_command_queue)
"""
Start streaming continuous data
"""
rospy.loginfo("Stopping the data stream")
if (False == self._continuous_data(False)):
rospy.logerr("Could not stop MOVO pan_tilt communication stream")
self.Shutdown()
return
"""
Start streaming continuous data
"""
rospy.loginfo("Starting the data stream")
if (False == self._continuous_data(True)):
rospy.logerr("Could not start MOVO pan_tilt communication stream")
self.Shutdown()
return
rospy.loginfo("Movo Pan-Tilt Head Driver is up and running")
self.init_success = True
#!/usr/bin/env python
import rospy
from trajectory_msgs.msg import JointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from geometry_msgs.msg import Twist
from gazebo_msgs.srv import *
from control_msgs.msg import JointTrajectoryControllerState
import math
from gazebo_msgs.msg import ModelState
import numpy as np
manipulator_state = JointTrajectoryControllerState()
got_msg = False
def callback(msg):
global got_msg
global manipulator_state
got_msg = True
manipulator_state = msg
def main():
rospy.init_node('manipulator_init')
manipulator_pub = rospy.Publisher('/manipulator_controller/command',
JointTrajectory,
queue_size=10)
rospy.Subscriber('/manipulator_controller/state',
JointTrajectoryControllerState, callback)
vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
get_state_service = rospy.ServiceProxy('/gazebo/get_model_state',
def __init__(self, topic='/arm_controller/state'):
self.state = JointTrajectoryControllerState()
rospy.Subscriber(topic,
JointTrajectoryControllerState,
self.callback,
queue_size=1)
def __init__(self, joint_names, arm, serial_num=""):
"""
Assume success until posted otherwise
"""
self.init_success = True
"""
Setup a lock for accessing data in the control loop
"""
self._lock = threading.Lock()
"""
Make sure the naming conventions and number of joints match what we expect
"""
self._joint_names = joint_names
self._num_joints = len(self._joint_names)
if (len(self._joint_names) != 6) and (len(self._joint_names) != 4):
rospy.logerr(
"Kinova products only support 4 or 6 DOF arms, caller passed %d...ABORTING"
% len(self._joint_names))
self.init_success = False
return
self._arm_name = arm
if (self._arm_name != 'right') and (self._arm_name != 'left'):
rospy.logerr(
"Must define arm as left or right, caller passed %s...ABORTING"
% self._arm_name)
self.init_success = False
return
"""
Create the hooks for the API
"""
self.kinova = CDLL('Kinova.API.USBCommandLayerUbuntu.so')
self.InitAPI = self.kinova.InitAPI
self.CloseAPI = self.kinova.CloseAPI
self.GetDevices = self.kinova.GetDevices
self.GetDevices.argtypes = [POINTER(KinovaDevice), POINTER(c_int)]
self.SetActiveDevice = self.kinova.SetActiveDevice
self.SetActiveDevice.argtypes = [KinovaDevice]
self.GetAngularPosition = self.kinova.GetAngularPosition
self.GetAngularPosition.argtypes = [POINTER(AngularPosition)]
self.GetAngularVelocity = self.kinova.GetAngularVelocity
self.GetAngularVelocity.argtypes = [POINTER(AngularPosition)]
self.GetAngularForce = self.kinova.GetAngularForce
self.GetAngularForce.argtypes = [POINTER(AngularPosition)]
self.StartControlAPI = self.kinova.StartControlAPI
self.SetAngularControl = self.kinova.SetAngularControl
self.StopControlAPI = self.kinova.StopControlAPI
self.SendAdvanceTrajectory = self.kinova.SendAdvanceTrajectory
self.SendAdvanceTrajectory.argtypes = [TrajectoryPoint]
self.SendBasicTrajectory = self.kinova.SendBasicTrajectory
self.SendBasicTrajectory.argtypes = [TrajectoryPoint]
self.EraseAllTrajectories = self.kinova.EraseAllTrajectories
self.SetAngularControl = self.kinova.SetAngularControl
self.DevInfoArrayType = (KinovaDevice * 20)
"""
Let the API try a few times to list devices, they take a few seconds to come up and
sometimes the API can fail if the caller hasn't waited for the hardware to come up
"""
attempts = 0
success = False
while not success and (attempts < 5) and not rospy.is_shutdown():
result1 = self.InitAPI()
result2 = c_int(0)
devinfo = self.DevInfoArrayType()
num_arms = self.GetDevices(devinfo, byref(result2))
if (NO_ERROR_KINOVA
== result1) and (NO_ERROR_KINOVA
== result2.value) and (num_arms > 0):
success = True
else:
rospy.logwarn("API failed to initialize.....Trying again.....")
self.CloseAPI()
attempts += 1
rospy.sleep(2)
if not success:
self.init_success = False
rospy.logerr("Init API result: %d" % result1)
rospy.logerr("GetDevices result: %d" % result2)
rospy.logerr("Number of arms: %d" % num_arms)
rospy.logerr(
"Initialization failed, could not find Kinova devices \
(see Kinova.API.CommLayerUbuntu.h for details)")
self.CloseAPI()
return
"""
Make sure there are devices available and that there is a device with the
serial number we are looking for
"""
found_arm = False
other_arms = []
for i in range(num_arms):
if (devinfo[i].SerialNumber == serial_num):
self._arm = devinfo[i]
rospy.loginfo("%s arm has serial number: %s" %
(self._arm_name, str(self._arm.SerialNumber)))
found_arm = True
else:
other_arms.append(str(devinfo[i].SerialNumber))
if not found_arm and ('' == serial_num):
rospy.logwarn(
"No serial number passed, using the first device in the list..."
)
self._arm = devinfo[0]
other_arms.pop(0)
rospy.loginfo("%s arm has serial number: %s" %
(self._arm_name, str(self._arm.SerialNumber)))
found_arm = True
if (len(other_arms) > 0):
rospy.logwarn(
"Found other arms not matching serial number parameter:")
for sn in other_arms:
rospy.logwarn(sn)
if not found_arm:
self.init_success = False
rospy.logerr("Could not find %s arm with serial number %s" %
(self._arm_name, serial_num))
rospy.logerr(
"Initialization failed; did not connect to desired arm...stopping driver"
)
self.CloseAPI()
self.init_failed = True
return
"""
Try and set the active device, the API version not matching is usually why this fails
"""
result = self.SetActiveDevice(self._arm)
if not (NO_ERROR_KINOVA == result):
rospy.logerr("Could not set %s arm active...stopping the driver" %
self._arm_name)
rospy.logerr("Set Active Device result: %d" % result)
rospy.logerr(
"Initialization failed, could not find Kinova devices \
(see Kinova.API.CommLayerUbuntu.h for details)")
self.StopControlAPI()
self.CloseAPI()
self.init_success = False
return
self._position_hold = False
"""
Initialize the PID controllers
"""
self._pid = [None] * self._num_joints
self._pid[0] = JacoPID(0.0, 0.0, 0.0)
self._pid[1] = JacoPID(0.0, 0.0, 0.0)
self._pid[2] = JacoPID(0.0, 0.0, 0.0)
self._pid[3] = JacoPID(0.0, 0.0, 0.0)
self._pid[4] = JacoPID(0.0, 0.0, 0.0)
#self._pid[5] = JacoPID(17.5,0.0,2.1)
#self._pid[5] = JacoPID(19.5,0.0,2.8)
self._pid[5] = JacoPID(17.5, 0.0, 2.1)
self._pid_error = [0.0] * self._num_joints
self._pid_output = [0.0] * self._num_joints
"""
Initialize the command interface, data processing and controller data
"""
self._raw_cmds = dict()
self._raw_cmds['position'] = self._get_angular_position()
self._raw_cmds['velocity'] = [0.0] * self._num_joints
self._raw_cmds['acceleration'] = [0.0] * self._num_joints
self._processed_cmds = dict()
self._processed_cmds['target'] = self._get_angular_position()
self._processed_cmds['rl_target'] = self._get_angular_position()
self._processed_cmds['ff_vel'] = [0.0] * self._num_joints
self._processed_cmds['ff_acc'] = [0.0] * self._num_joints
self._pid_input_filters = dict()
self._pid_input_filters['target'] = FilterSignals(
0.5, self._num_joints, self._get_angular_position())
self._pid_input_filters['ff_vel'] = FilterSignals(
2.0, self._num_joints, [0.0] * self._num_joints)
self._pid_input_filters['ff_acc'] = FilterSignals(
2.0, self._num_joints, [0.0] * self._num_joints)
self._pid_input_filters['rl_target'] = RateLimitSignals(
JOINT_VEL_LIMITS, self._num_joints, self._get_angular_position())
self._vel_diff = DifferentiateSignals(self._num_joints,
self._get_angular_position())
self._vff_diff = DifferentiateSignals(self._num_joints,
self._get_angular_position())
self._joint_fb = dict()
self._joint_fb['position'] = self._get_angular_position()
self._joint_fb['velocity'] = self._get_angular_velocity()
self._joint_fb['force'] = self._get_angular_force()
self._joint_fb_filters = dict()
self._joint_fb_filters['position'] = FilterSignals(
2.0, self._num_joints, self._get_angular_position())
self._joint_fb_filters['velocity'] = FilterSignals(
2.0, self._num_joints, [0.0] * self._num_joints)
self._joint_fb_filters['force'] = FilterSignals(
2.0, self._num_joints, self._get_angular_force())
"""
Register the publishers and subscribers
"""
self._jstpub = rospy.Publisher("/vector/%s_arm_controller/state" %
self._arm_name,
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("/vector/%s_arm/joint_states" %
self._arm_name,
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
"""
Update the feedback once to get things initialized
"""
self._update_controller_data()
"""
Start the controller
"""
rospy.loginfo("Starting the %s controller" % arm)
self._done = False
self._t1 = rospy.Timer(rospy.Duration(0.01), self._run_ctl)
def __init__(self,
prefix="",
gripper="",
interface='eth0',
jaco_ip="10.66.171.15",
dof=""):
"""
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
"""
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):
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]
"""
Register the publishers and subscribers
"""
self.last_teleop_cmd_update = rospy.get_time() - 0.5
self._teleop_cmd_sub = rospy.Subscriber(
"/movo/%s_arm/cartesian_vel_cmd" % self._prefix,
JacoCartesianVelocityCmd, self._update_teleop_cmd)
self._gripper_cmd = 0.0
self._ctl_mode = AUTONOMOUS_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._teleop_gripper_cmd_sub = rospy.Subscriber(
"/movo/%s_gripper/vel_cmd" % self._prefix, Float32,
self._update_teleop_gripper_cmd)
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
"""
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._pid[0] = JacoPID(5.0,0.0,0.8)
self._pid[1] = JacoPID(5.0,0.0,0.8)
self._pid[2] = JacoPID(5.0,0.0,0.8)
self._pid[3] = JacoPID(5.0,0.0,0.8)
self._pid[4] = JacoPID(5.0,0.0,0.8)
self._pid[5] = JacoPID(5.0,0.0,0.8)
"""
self.pause_controller = False
self._init_ext_joint_position_control()
self._init_ext_gripper_control()
"""
Set temporary tucked position after homing
"""
"""
if 'left' == self._prefix:
self._arm_cmds['position'][1]+= deg_to_rad(80.0)
self._arm_cmds['position'][2]+= deg_to_rad(50.0)
self._arm_cmds['position'][4]-= deg_to_rad(90.0)
else:
self._arm_cmds['position'][1]-= deg_to_rad(80.0)
self._arm_cmds['position'][2]-= deg_to_rad(50.0)
self._arm_cmds['position'][4]+= deg_to_rad(90.0)
"""
"""
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)
