def send_to_neutral():
"""
DON'T USE THIS ON REAL ROBOT!!!
"""
temp_pub = rospy.Publisher('/panda_simulator/motion_controller/arm/joint_commands',JointCommand, queue_size = 1, tcp_nodelay = True)
# Create JointCommand message to publish commands
pubmsg = JointCommand()
pubmsg.names = names # names of joints (has to be 7)
pubmsg.position = neutral_pose # JointCommand msg has other fields (velocities, efforts) for
# when controlling in other control mode
pubmsg.mode = pubmsg.POSITION_MODE # Specify control mode (POSITION_MODE, VELOCITY_MODE, IMPEDANCE_MODE (not available in sim), TORQUE_MODE)
curr_val = deepcopy(vals)
while all(abs(neutral_pose[i]-curr_val[i]) > 0.01 for i in range(len(curr_val))):
temp_pub.publish(pubmsg)
curr_val = deepcopy(vals)
def __init__(self):
# Subscribe to robot joint state
self._sub_js = rospy.Subscriber(
'/panda_simulator/custom_franka_state_controller/joint_states',
JointState, self._joint_states_cb)
# Subscribe to robot state (Refer JointState.msg to find all available data.
# Note: All msg fields are not populated when using the simulated environment)
self._sub_rs = rospy.Subscriber(
'/panda_simulator/custom_franka_state_controller/robot_state',
RobotState, self._robot_states_cb)
# Published joint command
self._pub = rospy.Publisher(
'/panda_simulator/motion_controller/arm/joint_commands',
JointCommand,
queue_size=1,
tcp_nodelay=True)
# Robot state
self.arm_joint_names = []
self.position = [0, 0, 0, 0, 0, 0, 0]
self.velocity = [0, 0, 0, 0, 0, 0, 0]
self.arm_dof = len(arm_joint_names)
# Create JointCommand message to publish commands
self.pubmsg = JointCommand()
self.pubmsg.names = arm_joint_names # names of joints (has to be 7 and in the same order as the command fields (positions, velocities, efforts))
self.pubmsg.mode = self.pubmsg.VELOCITY_MODE # Specify control mode (POSITION_MODE, VELOCITY_MODE, IMPEDANCE_MODE (not available in sim), TORQUE_MODE
# Internal Variables
self.go_pos = [1, 1, 1]
self.pos_limit = [0, 0, 0]
self.neg_limit = [0, 0, 0]
self.desired_velocity = [0, 0, 0, 0, 0, 0, 0]
self.filt = 0.49999
self.time = 0
def __init__(self, synchronous_pub=False):
self._params = RobotParams()
self._ns = self._params.get_base_namespace()
self._joint_limits = self._params.get_joint_limits()
joint_names = self._joint_limits.joint_names
if not joint_names:
rospy.logerr("{}: Cannot detect joint names for arm on this "
"robot. Exiting Arm.init().".format(
self.__class__.__name__))
return
self._joint_names = joint_names
self.name = self._params.get_robot_name()
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
self._stiffness_frame_effort = dict()
self._errors = dict()
self._collision_state = False
self._tip_states = None
self._jacobian = None
self._cartesian_contact = None
self._robot_mode = False
self._command_msg = JointCommand()
# neutral pose joint positions
self._neutral_pose_joints = self._params.get_neutral_pose()
self._frames_interface = FrankaFramesInterface()
try:
self._collision_behaviour_interface = CollisionBehaviourInterface()
except rospy.ROSException:
rospy.loginfo(
"{}: Collision Service Not found. It will not be possible to change collision behaviour of robot!"
.format(self.__class__.__name__))
self._collision_behaviour_interface = None
self._ctrl_manager = FrankaControllerManagerInterface(
ns=self._ns, sim=self._params._in_sim)
self._speed_ratio = 0.15
self._F_T_NE = np.eye(1).flatten().tolist()
self._NE_T_EE = np.eye(1).flatten().tolist()
queue_size = None if synchronous_pub else 1
with warnings.catch_warnings():
warnings.simplefilter("ignore")
self._joint_command_publisher = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=queue_size)
self._pub_joint_cmd_timeout = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_command_timeout',
Float64,
latch=True,
queue_size=10)
self._robot_state_subscriber = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/robot_state',
RobotState,
self._on_robot_state,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states'
self._joint_state_sub = rospy.Subscriber(joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
self._cartesian_state_sub = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/tip_state',
EndPointState,
self._on_endpoint_state,
queue_size=1,
tcp_nodelay=True)
rospy.on_shutdown(self._clean_shutdown)
err_msg = ("%s arm init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
franka_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current tip_state "
"from %s") % (self.name.capitalize(),
self._ns + 'tip_state')
franka_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current robot_state "
"from %s") % (self.name.capitalize(),
self._ns + 'robot_state')
franka_dataflow.wait_for(lambda: self._jacobian is not None,
timeout_msg=err_msg,
timeout=5.0)
# start moveit server with panda_link8 (flange) as the end-effector, unless it is in simulation. However, using move_to_cartesian_pose() method compensates for this and moves the robot's defined end-effector frame (EE frame; see set_EE_frame() and set_EE_at_frame()) to the target pose.
try:
self._movegroup_interface = PandaMoveGroupInterface(
use_panda_hand_link=True if self._params._in_sim else False)
except:
rospy.loginfo(
"{}: MoveGroup was not found! This is okay if moveit service is not required!"
.format(self.__class__.__name__))
self._movegroup_interface = None
self.set_joint_position_speed(self._speed_ratio)
def __init__(self, DS_type = 1, A = [0.0] * 7, DS_attractor = [0.0] * 7, ctrl_rate = 1000, cmd_type = 1, epsilon = 0.005, vel_limits = panda_maxVel, null_space = [0.0] * 7):
# Subscribe to robot joint state
self._sub_js = rospy.Subscriber('/panda_simulator/custom_franka_state_controller/joint_states', JointState,
self._joint_states_cb,
queue_size=1,tcp_nodelay=True)
# if not using franka_ros_interface, you have to subscribe to the right topics
# to obtain the current end-effector state and robot jacobian for computing
# commands
self._sub_cart_state = rospy.Subscriber('panda_simulator/custom_franka_state_controller/tip_state', EndPointState,
self._on_endpoint_state,
queue_size=1,tcp_nodelay=True)
self._sub_robot_state = rospy.Subscriber('panda_simulator/custom_franka_state_controller/robot_state',RobotState,
self._on_robot_state,
queue_size=1,tcp_nodelay=True)
# Published joint command
self._pub = rospy.Publisher('/panda_simulator/motion_controller/arm/joint_commands',JointCommand, queue_size = 1, tcp_nodelay = True)
self.DS_type = DS_type
if self.DS_type == 2:
self._pub_target = rospy.Publisher('DS_target', PointStamped)
# Robot state
self.arm_joint_names = []
self.position = [0,0,0,0,0,0,0]
self.velocity = [0,0,0,0,0,0,0]
self.arm_dof = len(arm_joint_names)
# Create JointCommand message to publish commands
self.cmd_type = cmd_type
self.pubmsg = JointCommand()
self.pubmsg.names = arm_joint_names # names of joints (has to be 7 and in the same order as the command fields (positions, velocities, efforts))
if self.cmd_type == 1:
self.pubmsg.mode = self.pubmsg.VELOCITY_MODE # Specify control mode (POSITION_MODE, VELOCITY_MODE, IMPEDANCE_MODE (not available in sim), TORQUE_MODE
elif self.cmd_type == 2:
self.pubmsg.mode = self.pubmsg.POSITION_MODE
# Robot Jacobian and EE representations
self.jacobian = []
self.ee_pose = []
self.ee_pos = []
self.ee_rot = []
self.ee_quat = []
self.ee_lin_vel = []
self.ee_ang_vel = []
# Control Variables
self.desired_velocity = [0,0,0,0,0,0,0]
self.desired_position = [0,0,0,0,0,0,0]
self.vel_limits = np.array(panda_maxVel)
self.add_nullspace = 0
# Variables for DS-Motion Generator
self.dt = 1.0/float(ctrl_rate)
self.DS_attractor = np.array(DS_attractor)
self.rate = rospy.Rate(ctrl_rate)
self.A = np.array(A)
self.epsilon = epsilon
self.pos_error = 0
self.quat_error = 0
self._stop = 0
self.force_limits = 0
# Spin once to update robot state
first_msg = rospy.wait_for_message('/joint_states', JointState)
self._populate_joint_state(first_msg)
# Given size of DS-attractor define FK and Jacobian functions
# Only for JT-DS type
if self.DS_type == 2:
self.DS_pos_att = self.DS_attractor[0:3]
if len(self.DS_attractor) == 3:
self.attr_type = 'pos'
if len(self.DS_attractor) == 7:
self.attr_type = 'full'
self.DS_quat_att = self.DS_attractor[3:7]
self.add_nullspace = 0
self.null_space = np.array(null_space)
self.S_limits = np.identity(self.arm_dof)
# For debugging purposes: Visualize init parameters for trajectory
rospy.loginfo('DS type (1:joint-space, 2: joint-space task oriented): {}'.format(self.DS_type))
# Specific DS parameters
if self.DS_type == 1:
rospy.loginfo('DS attractor: {}'.format(self.DS_attractor))
if self.DS_type == 2:
rospy.loginfo('DS position attractor: {}'.format(self.DS_pos_att))
rospy.loginfo('Null-Space target: {}'.format(self.null_space))
if len(self.DS_attractor) == 7:
rospy.loginfo('DS quaternion attractor: {}'.format(self.DS_quat_att))
# Generic DS parameters
rospy.loginfo('DS system matrix A:\n {}'.format(self.A))
rospy.loginfo('Stopping epsilon: {}'.format(self.epsilon))
# For debugging purposes: Visualize parameters for Motion Controllers
rospy.loginfo('Control command type (1: velocities, 2: positions): {}'.format(self.cmd_type))
rospy.loginfo("Sending robot to neutral pose")
send_to_neutral() # DON'T DO THIS ON REAL ROBOT!! (use move_to_neutral() method from ArmInterface of franka_ros_interface package)
rospy.sleep(2.0)
initial_pose = vals
rospy.loginfo("Commanding...\n")
elapsed_time_ = rospy.Duration(0.0)
period = rospy.Duration(0.005)
count = 0
# Create JointCommand message to publish commands
pubmsg = JointCommand()
pubmsg.names = names # names of joints (has to be 7 and in the same order as the command fields (positions, velocities, efforts))
while not rospy.is_shutdown():
elapsed_time_ += period
delta = 3.14 / 16.0 * (1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2
for j in range(len(vals)):
if j == 4:
vals[j] = initial_pose[j] - delta
else:
vals[j] = initial_pose[j] + delta
pubmsg.position = vals # JointCommand msg has other fields (velocities, efforts) for
# when controlling in other control mode
robot_state_sub = rospy.Subscriber(
'panda_simulator/custom_franka_state_controller/robot_state',
RobotState,
_on_robot_state,
queue_size=1,
tcp_nodelay=True)
desired_state_sub = rospy.Subscriber('panda_simulator/desired_twist',
Twist,
_on_desired_twist,
queue_size=1,
tcp_nodelay=True)
# create joint command message and fix its type to joint torque mode
command_msg = JointCommand()
command_msg.names = ['panda_joint1','panda_joint2','panda_joint3',\
'panda_joint4','panda_joint5','panda_joint6','panda_joint7']
command_msg.mode = JointCommand.TORQUE_MODE
# Also create a publisher to publish joint commands
joint_command_publisher = rospy.Publisher(
'panda_simulator/motion_controller/arm/joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=1)
# wait for messages to be populated before proceeding
rospy.loginfo("Subscribing to robot state topics...")
while (True):
if not (JACOBIAN is None or CARTESIAN_POSE is None):
def __init__(self,
DS_type=1,
A_p=[0.0] * 3,
A_o=[0.0] * 3,
DS_attractor=[0.0] * 7,
ctrl_rate=1000,
epsilon=0.005,
ctrl_orient=1,
learned_gamma=[],
pub_pose=1):
# Subscribe to robot joint state
self._sub_js = rospy.Subscriber(
'/panda_simulator/custom_franka_state_controller/joint_states',
JointState,
self._joint_states_cb,
queue_size=1,
tcp_nodelay=True)
# if not using franka_ros_interface, you have to subscribe to the right topics
# to obtain the current end-effector state and robot jacobian for computing
# commands
self._sub_cart_state = rospy.Subscriber(
'panda_simulator/custom_franka_state_controller/tip_state',
EndPointState,
self._on_endpoint_state,
queue_size=1,
tcp_nodelay=True)
self._sub_robot_state = rospy.Subscriber(
'panda_simulator/custom_franka_state_controller/robot_state',
RobotState,
self._on_robot_state,
queue_size=1,
tcp_nodelay=True)
# ---Publishes twist command to filter node
self._pub_twist = rospy.Publisher('/panda_simulator/desired_twist',
Twist,
queue_size=10)
self._pub_target = rospy.Publisher('DS_target',
PointStamped,
queue_size=1)
self._pub_ee_pos = rospy.Publisher('curr_ee_pos',
PointStamped,
queue_size=1)
self._pub_wrench = rospy.Publisher('/panda_simulator/desired_wrench',
WrenchStamped,
queue_size=10)
# Robot state
self.arm_joint_names = []
self.position = [0, 0, 0, 0, 0, 0, 0]
self.velocity = [0, 0, 0, 0, 0, 0, 0]
self.arm_dof = len(arm_joint_names)
self.pub_pose = pub_pose
# Create JointCommand message to publish commands
self._pubmsg = JointCommand()
self._pubmsg.names = arm_joint_names # names of joints (has to be 7 and in the same order as the command fields (positions, velocities, efforts))
self._pubmsg.mode = self._pubmsg.TORQUE_MODE
# Also create a publisher to publish joint commands
self._joint_command_publisher = rospy.Publisher(
'panda_simulator/motion_controller/arm/joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=1)
self.control_orient = ctrl_orient
# Robot Jacobian and EE representations
self.jacobian = []
self.ee_pose = []
self.ee_pos = []
self.ee_rot = []
self.ee_quat = []
self.ee_lin_vel = []
self.ee_ang_vel = []
# Robot commands in task-space
self.twist_msg = Twist()
self.twist_msg.linear.x = 0
self.twist_msg.linear.y = 0
self.twist_msg.linear.z = 0
self.twist_msg.angular.x = 0
self.twist_msg.angular.y = 0
self.twist_msg.angular.z = 0
# Might remove this, doesn't seem to work like I expect it to..
self.wrench_msg = WrenchStamped()
self.wrench_msg.wrench = Wrench()
self.wrench_msg.wrench.force = Vector3()
self.wrench_msg.wrench.torque = Vector3()
# self.wrench_msg.header.frame_id = '/panda_link7'
# self.wrench_msg.header.frame_id = '/world_on_ee'
self.wrench_msg.header.stamp = rospy.Time.now()
# Control Variables
self.desired_position = [0, 0, 0, 0, 0, 0]
self.desired_velocity = [0, 0, 0, 0, 0, 0]
# self.max_lin_vel = 1.70
# self.max_ang_vel = 2.5
self.max_lin_vel = 1.0
self.max_ang_vel = 1.0
# Variables for DS-Motion Generator
self.dt = 1.0 / float(ctrl_rate)
self.DS_type = DS_type
self.DS_attractor = np.array(DS_attractor)
self.rate = rospy.Rate(ctrl_rate)
self.epsilon = epsilon
self._stop = 0
# Linear DS
self.A_p = np.array(A_p)
self.pos_error = 0
self.scale_DS = 1
self.learned_gamma = learned_gamma
self.classifier = learned_gamma['classifier']
self.max_dist = learned_gamma['max_dist']
self.reference_points = learned_gamma['reference_points']
# Quaternion DS
self.A_o = np.array(A_o)
self.quat_error = 0
# Spin once to update robot state
first_msg = rospy.wait_for_message(
'/panda_simulator/custom_franka_state_controller/joint_states',
JointState)
self._populate_joint_state(first_msg)
self.DS_pos_att = self.DS_attractor[0:3]
if len(self.DS_attractor) == 3:
self.attr_type = 'pos'
if len(self.DS_attractor) == 7:
self.attr_type = 'full'
self.DS_quat_att = self.DS_attractor[3:7]
# For debugging purposes: Visualize init parameters for trajectory
rospy.loginfo('DS type (1:linear, 2: non-linear): {}'.format(
self.DS_type))
# Specific DS parameters
rospy.loginfo('DS position attractor: {}'.format(self.DS_pos_att))
rospy.loginfo('DS quaternion attractor: {}'.format(self.DS_quat_att))
# Generic DS parameters
rospy.loginfo('DS system matrix A_p:\n {}'.format(self.A_p))
rospy.loginfo('DS system matrix A_o:\n {}'.format(self.A_o))
rospy.loginfo('Stopping epsilon: {}'.format(self.epsilon))
#! /usr/bin/env python
import rospy
from franka_core_msgs.msg import JointCommand
if __name__ == "__main__":
rospy.init_node("force_neutral_pose_startup")
pub = rospy.Publisher(
'panda_simulator/motion_controller/arm/joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=10)
command_msg = JointCommand()
command_msg.names = ["panda_joint%d" % (idx) for idx in range(1, 8)]
command_msg.position = [0.000, -0.785, 0.0, -2.356, 0.0, 1.57, 0.785]
command_msg.mode = JointCommand.POSITION_MODE
rospy.sleep(0.5)
start = rospy.Time.now().to_sec()
rospy.loginfo("Attempting to force robot to neutral pose...")
rospy.sleep(0.5)
while not rospy.is_shutdown() and (rospy.Time.now().to_sec() - start < 1.):
# print rospy.Time.now()
command_msg.header.stamp = rospy.Time.now()
pub.publish(command_msg)
def __init__(self, synchronous_pub=False):
"""
Constructor.
@type synchronous_pub: bool
@param synchronous_pub: designates the JointCommand Publisher
as Synchronous if True and Asynchronous if False.
Synchronous Publishing means that all joint_commands publishing to
the robot's joints will block until the message has been serialized
into a buffer and that buffer has been written to the transport
of every current Subscriber. This yields predicable and consistent
timing of messages being delivered from this Publisher. However,
when using this mode, it is possible for a blocking Subscriber to
prevent the joint_command functions from exiting. Unless you need exact
JointCommand timing, default to Asynchronous Publishing (False).
"""
self._params = RobotParams()
self._ns = self._params.get_base_namespace()
self._joint_limits = self._params.get_joint_limits()
joint_names = self._joint_limits.joint_names
if not joint_names:
rospy.logerr("Cannot detect joint names for arm on this "
"robot. Exiting Arm.init().")
return
self._joint_names = joint_names
self.name = self._params.get_robot_name()
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
self._stiffness_frame_effort = dict()
self._errors = dict()
self._collision_state = False
self._tip_states = None
self._jacobian = None
self._cartesian_contact = None
self._robot_mode = False
self._command_msg = JointCommand()
# neutral pose joint positions
self._neutral_pose_joints = self._params.get_neutral_pose()
self._frames_interface = FrankaFramesInterface()
self._ctrl_manager = FrankaControllerManagerInterface(
ns=self._ns, sim=True if self._params._in_sim else False)
self._speed_ratio = 0.15
queue_size = None if synchronous_pub else 1
with warnings.catch_warnings():
warnings.simplefilter("ignore")
self._joint_command_publisher = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=queue_size)
self._pub_joint_cmd_timeout = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_command_timeout',
Float64,
latch=True,
queue_size=10)
self._robot_state_subscriber = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/robot_state',
RobotState,
self._on_robot_state,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states' if not self._params._in_sim else self._ns + '/joint_states'
self._joint_state_sub = rospy.Subscriber(joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
self._cartesian_state_sub = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/tip_state',
EndPointState,
self._on_endpoint_state,
queue_size=1,
tcp_nodelay=True)
rospy.on_shutdown(self._clean_shutdown)
err_msg = ("%s arm init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
franka_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current tip_state "
"from %s") % (self.name.capitalize(),
self._ns + 'tip_state')
franka_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current robot_state "
"from %s") % (self.name.capitalize(),
self._ns + 'robot_state')
franka_dataflow.wait_for(lambda: self._jacobian is not None,
timeout_msg=err_msg,
timeout=5.0)
def __init__(self, synchronous_pub=False):
"""
"""
self.hand = franka_interface.GripperInterface()
self._params = RobotParams()
self._ns = self._params.get_base_namespace()
self._joint_limits = self._params.get_joint_limits()
joint_names = self._joint_limits.joint_names
if not joint_names:
rospy.logerr("Cannot detect joint names for arm on this "
"robot. Exiting Arm.init().")
return
self._joint_names = joint_names
self.name = self._params.get_robot_name()
self._joint_angle = dict()
self._joint_velocity = dict()
self._joint_effort = dict()
self._cartesian_pose = dict()
self._cartesian_velocity = dict()
self._cartesian_effort = dict()
self._stiffness_frame_effort = dict()
self._errors = dict()
self._collision_state = False
self._tip_states = None
self._jacobian = None
self._cartesian_contact = None
self._robot_mode = False
self._command_msg = JointCommand()
# neutral pose joint positions
self._neutral_pose_joints = self._params.get_neutral_pose()
self._frames_interface = FrankaFramesInterface()
try:
self._collision_behaviour_interface = CollisionBehaviourInterface()
except rospy.ROSException:
rospy.loginfo(
"Collision Service Not found. It will not be possible to change collision behaviour of robot!"
)
self._collision_behaviour_interface = None
self._ctrl_manager = FrankaControllerManagerInterface(
ns=self._ns, sim=self._params._in_sim)
self._speed_ratio = 0.15
queue_size = None if synchronous_pub else 1
with warnings.catch_warnings():
warnings.simplefilter("ignore")
self._joint_command_publisher = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_commands',
JointCommand,
tcp_nodelay=True,
queue_size=queue_size)
self._pub_joint_cmd_timeout = rospy.Publisher(
self._ns + '/motion_controller/arm/joint_command_timeout',
Float64,
latch=True,
queue_size=10)
self._robot_state_subscriber = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/robot_state',
RobotState,
self._on_robot_state,
queue_size=1,
tcp_nodelay=True)
joint_state_topic = self._ns + '/custom_franka_state_controller/joint_states'
self._joint_state_sub = rospy.Subscriber(joint_state_topic,
JointState,
self._on_joint_states,
queue_size=1,
tcp_nodelay=True)
self._cartesian_state_sub = rospy.Subscriber(
self._ns + '/custom_franka_state_controller/tip_state',
EndPointState,
self._on_endpoint_state,
queue_size=1,
tcp_nodelay=True)
# Cartesian Impedance Controller Publishers
self._cartesian_impedance_pose_publisher = rospy.Publisher(
"equilibrium_pose", PoseStamped, queue_size=10)
self._cartesian_stiffness_publisher = rospy.Publisher(
"impedance_stiffness", CartImpedanceStiffness, queue_size=10)
# Force Control Publisher
self._force_controller_publisher = rospy.Publisher("wrench_target",
Wrench,
queue_size=10)
# Torque Control Publisher
self._torque_controller_publisher = rospy.Publisher("torque_target",
TorqueCmd,
queue_size=20)
# Joint Impedance Controller Publishers
self._joint_impedance_publisher = rospy.Publisher(
"joint_impedance_position_velocity", JICmd, queue_size=20)
self._joint_stiffness_publisher = rospy.Publisher(
"joint_impedance_stiffness",
JointImpedanceStiffness,
queue_size=10)
rospy.on_shutdown(self._clean_shutdown)
err_msg = ("%s arm init failed to get current joint_states "
"from %s") % (self.name.capitalize(), joint_state_topic)
franka_dataflow.wait_for(lambda: len(self._joint_angle.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current tip_state "
"from %s") % (self.name.capitalize(),
self._ns + 'tip_state')
franka_dataflow.wait_for(lambda: len(self._cartesian_pose.keys()) > 0,
timeout_msg=err_msg,
timeout=5.0)
err_msg = ("%s arm, init failed to get current robot_state "
"from %s") % (self.name.capitalize(),
self._ns + 'robot_state')
franka_dataflow.wait_for(lambda: self._jacobian is not None,
timeout_msg=err_msg,
timeout=5.0)
self.set_joint_position_speed(self._speed_ratio)
