def joint_state_publisher(self):
try:
joint_state_msg = JointState()
joint_fb_msg = FollowJointTrajectoryFeedback()
time = rospy.Time.now()
with self.lock:
#Joint states
joint_state_msg.header.stamp = time
joint_state_msg.name = self.joint_names
joint_state_msg.position = self.motion_ctrl.get_joint_positions()
#self.joint_state_pub.publish(joint_state_msg)
#Joint feedback
joint_fb_msg.header.stamp = time
joint_fb_msg.joint_names = self.joint_names
joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions()
#self.joint_feedback_pub.publish(joint_fb_msg)
except Exception as e:
rospy.logerr('Unexpected exception in joint state publisher: %s', e)
def feedback_states_publisher(rate):
global current_joints
global current_velocity
publisher = rospy.Publisher('feedback_states', FollowJointTrajectoryFeedback, queue_size = conf_rate)
sleeper = rospy.Rate(rate)
while not rospy.is_shutdown():
msg = FollowJointTrajectoryFeedback()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = conf_base_name
msg.joint_names = conf_joint_names
msg.actual = JointTrajectoryPoint()
# -----------------------------------------------------------
# Critical Section
# -----------------------------------------------------------
with lock:
msg.actual.positions = current_joints
msg.actual.velocities = current_velocity
msg.actual.effort = current_effort
msg.actual.time_from_start = rospy.get_rostime() - trajectory_start
# -----------------------------------------------------------
publisher.publish(msg)
sleeper.sleep()
def _state_publisher(self):
try:
joint_state_msg = JointState()
joint_fb_msg = FollowJointTrajectoryFeedback()
with self._lock:
time = rospy.Time.now()
q_actual, q_desired, q_error = self._rsi_connection.get_joint_states(
)
#Joint states
joint_state_msg.header.stamp = time
joint_state_msg.name = self._joint_names
joint_state_msg.position = q_actual
self._joint_state_pub.publish(joint_state_msg)
#Joint feedback
joint_fb_msg.header.stamp = time
joint_fb_msg.joint_names = self._joint_names
joint_fb_msg.actual.positions = q_actual
joint_fb_msg.desired.positions = q_desired
joint_fb_msg.error.positions = q_error
self._joint_feedback_pub.publish(joint_fb_msg)
except Exception as e:
rospy.logerr('Unexpected exception in joint state publisher: %s',
e)
def joint_state_publisher(self):
joint_state_msg = JointState()
joint_state_msg.header.stamp = rospy.Time.now()
joint_state_msg.name = self.ABB_joint_name_list + self.gripper3F_joint_name_list
joint_state_msg.position = list(self.ABB_joint_states_list) + list(
self.gripper3F_joint_states_list)
joint_state_msg.velocity = []
joint_state_msg.effort = []
control_state_msg = FollowJointTrajectoryFeedback()
control_state_msg.header.stamp = rospy.Time.now()
control_state_msg.joint_names = self.ABB_joint_name_list + self.gripper3F_joint_name_list
control_state_msg.actual.positions = list(
self.ABB_feedback_states_list) + list(
self.gripper3F_feedback_states_list)
control_state_msg.desired.positions = list(
self.ABB_feedback_states_list) + list(
self.gripper3F_feedback_states_list)
control_state_msg.error.positions = [
0 for i in control_state_msg.joint_names
]
self.joint_state_pub.publish(joint_state_msg)
self.control_state_pub.publish(control_state_msg)
def joint_state_publisher(self):
if self.EnableFlag == 1 and self.motion_ctrl.positionUpdatedFlag == '1':
try:
joint_state_msg = JointState()
joint_fb_msg = FollowJointTrajectoryFeedback()
time = rospy.Time.now()
with self.lock:
#Joint states
joint_state_msg.header.stamp = time
joint_state_msg.name = self.joint_names
joint_state_msg.position = self.motion_ctrl.get_joint_positions(
)
# self.joint_state_pub.publish(joint_state_msg)
#Joint feedback
joint_fb_msg.header.stamp = time
joint_fb_msg.joint_names = self.joint_names
joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions(
)
self.joint_feedback_pub.publish(joint_fb_msg)
except Exception as e:
rospy.loginfo(
'Unexpected exception in joint state publisher: %s', e)
def execute_cb(self, goal):
rospy.loginfo("Received new goal:\n%s"%goal)
success = True
start_time = rospy.Time.now()
i = 1
previouspoint = None
for point in goal.trajectory.points:
time_to_wait = start_time + point.time_from_start - rospy.Time.now()
#previouspoint=point
if time_to_wait > rospy.Duration(0):
#rospy.loginfo("Sleeping for %s seconds..."%time_to_wait.to_sec())
rospy.loginfo("Sleeping for 0.3 second")
rospy.sleep(0.3)
# only publish feedback if we have received some position recently
if previouspoint and self.latestpositions:
fb = FollowJointTrajectoryFeedback()
fb.desired = previouspoint
fb.actual = JointTrajectoryPoint()
fb.actual.positions = self.latestpositions
fb.error = JointTrajectoryPoint()
fb.error.positions = map(sub, fb.desired.positions, fb.actual.positions)
self.action_server.publish_feedback(fb)
# only use positions and velocities (velocities are always positive)
point.velocities = map(abs, point.velocities)
point.accelerations = []
point.effort = []
rospy.loginfo("Go to point %d:\n%s"%(i, point))
serial_command=Float64MultiArray()
previous_position=Float64MultiArray()
#serial_command.data= tuple(numpy.subtract(point.positions,previouspoint.positions))
serial_command.data=point.positions
self.pub_command.publish(serial_command)
self.latestpositions = None
self.pub_trajpoint.publish(point)
previouspoint = point
i += 1
if success:
rospy.loginfo('%s: Succeeded' % self.action_name)
self.result = FollowJointTrajectoryResult()
self.result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self.action_server.set_succeeded(self.result)
self.traj_complete.publish(1)
def read_joint_state_data(self, msg):
joint_fb_msg = FollowJointTrajectoryFeedback()
joint_fb_msg.header.stamp = rospy.Time.now()
joint_fb_msg.joint_names = msg.name
joint_fb_msg.actual.positions = msg.position
joint_fb_msg.actual.velocities = msg.velocity
self.joint_feedback_pub.publish(joint_fb_msg)
def on_goal(self, goal_handle):
"""Handle a new goal trajectory command."""
# Checks if the joints are just incorrect
if set(goal_handle.get_goal().trajectory.joint_names) != set(
self.prefixedJointNames):
rospy.logerr(
"Received a goal with incorrect joint names: (%s)" %
', '.join(goal_handle.get_goal().trajectory.joint_names))
goal_handle.set_rejected()
return
if not trajectory_is_finite(goal_handle.get_goal().trajectory):
rospy.logerr("Received a goal with infinites or NaNs")
goal_handle.set_rejected(
text="Received a goal with infinites or NaNs")
return
# Checks that the trajectory has velocities
if not has_velocities(goal_handle.get_goal().trajectory):
rospy.logerr("Received a goal without velocities")
goal_handle.set_rejected(text="Received a goal without velocities")
return
feedback = FollowJointTrajectoryFeedback()
result = FollowJointTrajectoryResult()
feedback.joint_names = goal_handle.get_goal().trajectory.joint_names
# Orders the joints of the trajectory according to joint_names
reorder_trajectory_joints(goal_handle.get_goal().trajectory,
self.prefixedJointNames)
# Inserts the current setpoint at the head of the trajectory
now = self.robot.getTime()
print 'trajectory_t0: {}'.format(self.trajectory_t0)
point0 = sample_trajectory(self.trajectory, now - self.trajectory_t0)
point0.time_from_start = rospy.Duration(0.0)
goal_handle.get_goal().trajectory.points.insert(0, point0)
feedback.desired.positions = point0
self.trajectory_t0 = now
print "i am here"
print 'point0: {}'.format(point0.positions)
# Replaces the goal
self.goal_handle = goal_handle
self.trajectory = goal_handle.get_goal().trajectory
self.feedback = feedback
self.result = result
goal_handle.set_accepted()
def __init__(self, limb, reconfig_server, rate=100.0):
self._dyn = reconfig_server
self._ns = 'robot/limb/' + limb + '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._action_name = rospy.get_name()
self._server.start()
self._limb = baxter_interface.Limb(limb)
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments and dynamic reconfigure
self._control_rate = rate # Hz
self._control_joints = []
self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
self._goal_time = 0.0
self._goal_error = dict()
self._error_threshold = dict()
self._dflt_vel = dict()
# Create our PID controllers
self._pid = dict()
for joint in self._limb.joint_names():
self._pid[joint] = baxter_control.PID()
# Set joint state publishing to specified control rate
self._pub_rate = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
self._pub_rate.publish(self._control_rate)
def initialize(self):
# subscribe to joint states to check if the arm is moving, based on velocities of each joint
rospy.Subscriber("/joint_states", JointState, self.jointStatesCallback, queue_size=1)
ns = self.controller_namespace + '/joint_trajectory_action_node/constraints'
self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0)
self.stopped_velocity_tolerance = rospy.get_param(ns + '/stopped_velocity_tolerance', 0.01)
self.goal_constraints = []
self.trajectory_constraints = []
self.min_velocity = rospy.get_param(self.controller_namespace + '/joint_trajectory_action_node/min_velocity', 0.1)
for joint in self.joint_names:
self.goal_constraints.append(rospy.get_param(ns + '/' + joint + '/goal', -1.0))
self.trajectory_constraints.append(rospy.get_param(ns + '/' + joint + '/trajectory', -1.0))
# Message containing current state for all controlled joints
self.msg = FollowJointTrajectoryFeedback()
self.msg.joint_names = self.joint_names
self.msg.desired.positions = [0.0] * self.num_joints
self.msg.desired.velocities = [0.0] * self.num_joints
self.msg.desired.accelerations = [0.0] * self.num_joints
self.msg.actual.positions = [0.0] * self.num_joints
self.msg.actual.velocities = [0.0] * self.num_joints
self.msg.error.positions = [0.0] * self.num_joints
self.msg.error.velocities = [0.0] * self.num_joints
return True
def __init__(self,
limb,
reconfig_server,
rate=100.0,
mode='position_w_id',
interpolation='minjerk'):
self._dyn = reconfig_server
self._ns = 'robot/limb/' + limb
self._fjt_ns = self._ns + '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._fjt_ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._action_name = rospy.get_name()
self._limb = intera_interface.Limb(limb, synchronous_pub=True)
self._enable = intera_interface.RobotEnable()
self._name = limb
self._interpolation = interpolation
self._cuff = intera_interface.Cuff(limb=limb)
# Verify joint control mode
self._mode = mode
if (self._mode != 'position' and self._mode != 'position_w_id'
and self._mode != 'velocity'):
rospy.logerr("%s: Action Server Creation Failed - "
"Provided Invalid Joint Control Mode '%s' (Options: "
"'position_w_id', 'position', 'velocity')" % (
self._action_name,
self._mode,
))
return
self._server.start()
self._alive = True
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments, messages, and dynamic
# reconfigure
self._control_rate = rate # Hz
self._control_joints = []
self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
self._goal_time = 0.0
self._stopped_velocity = 0.0
self._goal_error = dict()
self._path_thresh = dict()
# Create our PID controllers
self._pid = dict()
for joint in self._limb.joint_names():
self._pid[joint] = intera_control.PID()
# Create our spline coefficients
self._coeff = [None] * len(self._limb.joint_names())
# Set joint state publishing to specified control rate
self._pub_rate = rospy.Publisher('/robot/joint_state_publish_rate',
UInt16,
queue_size=10)
self._pub_rate.publish(self._control_rate)
def initialize(self):
ns = self.controller_namespace + '/joint_trajectory_action_node/constraints'
self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0)
self.stopped_velocity_tolerance = rospy.get_param(
ns + '/stopped_velocity_tolerance', 0.01)
self.goal_constraints = []
self.trajectory_constraints = []
self.min_velocity = rospy.get_param(
self.controller_namespace +
'/joint_trajectory_action_node/min_velocity', 0.1)
# Topic name for publish joint_states
self.joint_states_topic = rospy.get_param(
self.controller_namespace +
'/joint_trajectory_action_node/joint_states_topic',
'/joint_states')
for joint in self.joint_names:
self.goal_constraints.append(
rospy.get_param(ns + '/' + joint + '/goal', -1.0))
self.trajectory_constraints.append(
rospy.get_param(ns + '/' + joint + '/trajectory', -1.0))
# Message containing current state for all controlled joints
self.msg = FollowJointTrajectoryFeedback()
self.msg.joint_names = self.joint_names
self.msg.desired.positions = [0.0] * self.num_joints
self.msg.desired.velocities = [0.0] * self.num_joints
self.msg.desired.accelerations = [0.0] * self.num_joints
self.msg.actual.positions = [0.0] * self.num_joints
self.msg.actual.velocities = [0.0] * self.num_joints
self.msg.error.positions = [0.0] * self.num_joints
self.msg.error.velocities = [0.0] * self.num_joints
return True
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 __init__(self, reconfig_server, rate=100.0, interpolation='minjerk'):
self._dyn = reconfig_server
self._fjt_ns = "position_joint_trajectory_controller/follow_joint_trajectory"
self._server = actionlib.SimpleActionServer(
self._fjt_ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._arm = franka_interface.ArmInterface(synchronous_pub=True)
self._enable = franka_interface.RobotEnable()
self._interpolation = interpolation
# Verify joint control mode
self._server.start()
self._alive = True
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments, messages, and dynamic
# reconfigure
self._control_rate = rate # Hz
self._control_joints = []
self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
self._goal_time = 0.0
self._stopped_velocity = 0.0001
self._goal_error = dict()
self._path_thresh = dict()
# Create our PID controllers
self._pid = dict()
for joint in self._arm.joint_names():
self._pid[joint] = pid.PID()
# Create our spline coefficients
self._coeff = [None] * len(self._arm.joint_names())
def __init__(self, topic_in):
pub_topic = rospy.get_param(node_name + '/pub_topic',
'/feedback_states')
rospy.Subscriber(topic_in, JointState, self._callBack)
self.joint_trajectory_pub = rospy.Publisher(
pub_topic, FollowJointTrajectoryFeedback, queue_size=1)
self.follow_joint_trajectory_feedback_ = FollowJointTrajectoryFeedback(
)
def joint_state_publisher(self):
LOG.info(
"QWERQWERQWERQWEQWRQWERQWERWQERQWERQWERQWERQWERQWERERQWQREWQWREQWERQWER!"
)
print('comecomecomecomecomecomecomecomecomecomecome')
joint_state_msg = JointState()
joint_state_msg.header.stamp = rospy.Time.now()
if self.robot_name == 'NRMK-IndyRP2':
joint_state_msg.name = [
'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5',
'joint6'
]
else:
joint_state_msg.name = [
'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5'
]
cur_joint_pos = self.indy.get_joint_pos()
joint_state_msg.position = utils_transf.degs2rads(cur_joint_pos)
joint_state_msg.velocity = [self.vel]
joint_state_msg.effort = []
control_state_msg = FollowJointTrajectoryFeedback()
control_state_msg.header.stamp = rospy.Time.now()
if self.robot_name == 'NRMK-IndyRP2':
control_state_msg.joint_names = [
'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5',
'joint6'
]
else:
control_state_msg.joint_names = [
'joint0', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5'
]
control_state_msg.actual.positions = utils_transf.degs2rads(
cur_joint_pos)
control_state_msg.desired.positions = utils_transf.degs2rads(
cur_joint_pos)
control_state_msg.error.positions = [
0 for i in control_state_msg.joint_names
]
self.joint_state_pub.publish(joint_state_msg)
self.control_state_pub.publish(control_state_msg)
def __init__(self, node):
self.node = node
self.server = actionlib.SimpleActionServer(
'/stretch_controller/follow_joint_trajectory',
FollowJointTrajectoryAction,
execute_cb=self.execute_cb,
auto_start=False)
self.feedback = FollowJointTrajectoryFeedback()
self.result = FollowJointTrajectoryResult()
r = self.node.robot
head_pan_range_ticks = r.head.motors['head_pan'].params['range_t']
head_pan_range_rad = (r.head.motors['head_pan'].ticks_to_world_rad(
head_pan_range_ticks[1]),
r.head.motors['head_pan'].ticks_to_world_rad(
head_pan_range_ticks[0]))
head_tilt_range_ticks = r.head.motors['head_tilt'].params['range_t']
head_tilt_range_rad = (r.head.motors['head_tilt'].ticks_to_world_rad(
head_tilt_range_ticks[1]),
r.head.motors['head_tilt'].ticks_to_world_rad(
head_tilt_range_ticks[0]))
wrist_yaw_range_ticks = r.end_of_arm.motors['wrist_yaw'].params[
'range_t']
wrist_yaw_range_rad = (
r.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(
wrist_yaw_range_ticks[1]),
r.end_of_arm.motors['wrist_yaw'].ticks_to_world_rad(
wrist_yaw_range_ticks[0]))
gripper_range_ticks = r.end_of_arm.motors['stretch_gripper'].params[
'range_t']
gripper_range_rad = (
r.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(
gripper_range_ticks[0]),
r.end_of_arm.motors['stretch_gripper'].ticks_to_world_rad(
gripper_range_ticks[1]))
gripper_range_robotis = (
r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(
gripper_range_rad[0]),
r.end_of_arm.motors['stretch_gripper'].world_rad_to_pct(
gripper_range_rad[1]))
self.head_pan_cg = HeadPanCommandGroup(
head_pan_range_rad, self.node.head_pan_calibrated_offset_rad,
self.node.head_pan_calibrated_looked_left_offset_rad)
self.head_tilt_cg = HeadTiltCommandGroup(
head_tilt_range_rad, self.node.head_tilt_calibrated_offset_rad,
self.node.head_tilt_calibrated_looking_up_offset_rad,
self.node.head_tilt_backlash_transition_angle_rad)
self.wrist_yaw_cg = WristYawCommandGroup(wrist_yaw_range_rad)
self.gripper_cg = GripperCommandGroup(gripper_range_robotis)
self.telescoping_cg = TelescopingCommandGroup(
tuple(r.arm.params['range_m']),
self.node.wrist_extension_calibrated_retracted_offset_m)
self.lift_cg = LiftCommandGroup(tuple(r.lift.params['range_m']))
self.mobile_base_cg = MobileBaseCommandGroup(virtual_range_m=(-0.5,
0.5))
def execute_cb_arm(self, goal):
# create feedback msg
self.arm_feedback = FollowJointTrajectoryFeedback()
self.arm_feedback.joint_names = goal.trajectory.joint_names
## notice that the order in goal.trajectory.joint_names is different
## from the default order in self.joint_names!!!!
#~ print goal.trajectory.joint_names
print "arm"
#~ print len(goal.trajectory.points)
print goal.trajectory.points
# we execute the trajectory at 5Hz, which means we execute 5
# points per second
action_rate = rospy.Rate(5)
## for each point in the planned trajectory, we excute it through
## SPI and send feedback
for trajectory_point in goal.trajectory.points:
# the actual jointstate is same as the required jointstate
self.arm_feedback.actual = trajectory_point
# we send the feedback infomation back to the client
self._as_arm.publish_feedback(self.arm_feedback)
# send the jointstates through SPI in the correct order !!!!
for i in range(len(self.jointStates) - 1):
self.jointStates[i] = trajectory_point.positions[
goal.trajectory.joint_names.index(self.joint_names[i])]
print self.jointStates
########################## IMPORTANT #######################
### add your own linear mapping between self.jointStates and self.jointPWMs
### self.jointPWWs = f(self.jointStates)
self.jointPWMs = np.array(self.k) * np.array(
self.jointStates) + np.array(self.b)
print self.jointPWMs
############################################################
# we publish the updated joint states, with the correct time stamps
self.joint_state_msg.header.stamp = rospy.Time.now()
self.joint_state_msg.position = self.jointStates
self.pub_state.publish(self.joint_state_msg)
self.joint_PWM_msg.position = self.jointPWMs
self.pub_PWM.publish(self.joint_PWM_msg)
# send PWMs to SPI
# uncomment this if you are using real hardware and sending
# data to Arduino through SPI
self.spi_send_PWM()
action_rate.sleep()
# After executing every trajectory point sent by the client
# we notify the client that the action is being completed.
self.arm_result = FollowJointTrajectoryResult()
self.arm_result.error_code = 0 # error code 0 means successful
self._as_arm.set_succeeded(self.arm_result)
def tr2_arm_follow_joint_trajectory(goal):
global tr2_arm_action_server, tr2_a0_state, tr2_a1_state, tr2_a2_state, tr2_a3_state, tr2_a4_state, tr2_a0_pub, tr2_a1_pub, tr2_a2_pub, tr2_a3_pub, tr2_a4_pub, tr2_mode_servo_pub, tr2_stop_pub
tr2_mode_servo_pub.publish(1)
tr2_stop_pub.publish(0)
time.sleep(0.050)
success = True
feedback = FollowJointTrajectoryFeedback()
result = FollowJointTrajectoryResult()
joint_names = goal.trajectory.joint_names
feedback.joint_names = joint_names
print len(goal.trajectory.points)
t_start = datetime.datetime.now()
for point in goal.trajectory.points:
tr2_a0_pub.publish(point.positions[0])
tr2_a1_pub.publish(point.positions[1])
tr2_a2_pub.publish(point.positions[2])
tr2_a3_pub.publish(point.positions[3])
tr2_a4_pub.publish(point.positions[4])
'''if tr2_arm_action_server.is_preempt_requested():
tr2_arm_action_server.set_preempted()
success = False
break'''
while (datetime.datetime.now() - t_start
).total_seconds() < point.time_from_start.nsecs / 1000000000.0:
feedback.desired = point
feedback.actual.positions = [
tr2_a0_state, tr2_a1_state, tr2_a2_state, tr2_a3_state,
tr2_a4_state
]
#feedback.error.positions
tr2_arm_action_server.publish_feedback(feedback)
tr2_arm_action_server.set_succeeded(result)
time.sleep(5)
tr2_stop_pub.publish(1)
def execute_cb(self, goal):
rospy.loginfo("Received new goal:\n%s"%goal)
success = True
start_time = rospy.Time.now()
i = 1
previouspoint = None
for point in goal.trajectory.points:
time_to_wait = start_time + point.time_from_start - rospy.Time.now()
if time_to_wait > rospy.Duration(0):
rospy.loginfo("Sleeping for %s seconds..."%time_to_wait.to_sec())
rospy.sleep(time_to_wait)
# only publish feedback if we have received some position recently
if previouspoint and self.latestpositions:
fb = FollowJointTrajectoryFeedback()
fb.desired = previouspoint
fb.actual = JointTrajectoryPoint()
fb.actual.positions = self.latestpositions
fb.error = JointTrajectoryPoint()
fb.error.positions = map(sub, fb.desired.positions, fb.actual.positions)
self.action_server.publish_feedback(fb)
# only use positions and velocities (velocities are always positive)
point.velocities = map(abs, point.velocities)
point.accelerations = []
point.effort = []
rospy.loginfo("Go to point %d:\n%s"%(i, point))
self.latestpositions = None
self.pub_trajpoint.publish(point)
previouspoint = point
i += 1
if success:
rospy.loginfo('%s: Succeeded' % self.action_name)
self.result = FollowJointTrajectoryResult()
self.result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self.action_server.set_succeeded(self.result)
def __init__(self, controller_name):
self._action_ns = controller_name + '/follow_joint_trajectory'
self._as = actionlib.SimpleActionServer(self._action_ns,
FollowJointTrajectoryAction,
execute_cb=self._execute_cb,
auto_start=False)
self._action_name = rospy.get_name()
self._as.start()
self._feedback = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
self._rate = rospy.Rate(20)
rospy.loginfo('Successful init')
def __init__(self, reconfig_server, rate=100.0):
self._dyn = reconfig_server
self.continuous = self._dyn.config['continuous']
self._fjt = '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._fjt,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._action_name = rospy.get_name()
# Current joint states
self._positions = {}
self._velocities = {}
# Actual robot control
self.states = EdoStates()
rospy.Subscriber("joint_states", JointState, self._js_callback)
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments, messages, and dynamic reconfigure
self._control_rate = rate # Hz
self._update_rate_spinner = rospy.Rate(self._control_rate)
self._control_joints = []
self._goal_time = 0.0
self._stopped_velocity = 0.0
self._goal_error = dict()
self._path_thresh = dict()
# Start the action server
rospy.sleep(0.5)
while True:
if self.states.edo_current_state in [
self.states.CS_CALIBRATED, self.states.CS_BRAKED
]:
self._server.start()
self._alive = True
rospy.loginfo("Trayectory action server started")
break
else:
rospy.logerr(
"Joint Trajectory Action Server cannot be started when robot is in state %s"
% self.states.get_current_code_string())
rospy.logerr(
"Make sure your robot is started and calibrated properly with calibrate.launch"
)
rospy.logerr("Trying again in 1 second... ")
rospy.sleep(1)
def __init__(self, limb, reconfig_server, rate=100.0,
mode='joint_impedance', sim = False, interpolation='minjerk'):
self._mode = mode
self._dyn = reconfig_server
self._ns = limb + '/joint_trajectory_controller'
self._fjt_ns = self._ns + '/follow_joint_trajectory2'
self._server = actionlib.SimpleActionServer(
self._fjt_ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._action_name = rospy.get_name()
self._limb = BorisRobot()
self._name = limb
self._interpolation = interpolation
self._server.start()
self._alive = True
# Action Feedback/Result
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
# Controller parameters from arguments, messages, and dynamic
# reconfigure
self._control_rate = rate # Hz
self._control_joints = []
self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
self._goal_time = 0.0
self._stopped_velocity = 0.0
self._goal_error = dict()
self._path_thresh = dict()
self._limb.set_control_mode(mode=self._mode)
if sim:
rospy.loginfo("Setting gains for simulated robot")
self._limb.set_joint_impedance(np.array([2000,2000,2000,2000,2000,2000,2000]),np.array([100,100,100,100,100,100,100]))
# Create our spline coefficients
self._coeff = [None] * len(self._limb.joint_names(self._name))
# Set joint state publishing to specified control rate
self._pub_rate = rospy.Publisher(
'/robot/joint_state_publish_rate',
UInt16,
queue_size=10)
self._pub_rate.publish(self._control_rate)
def _set_current_joint_angles(self, joint_angles):
self._current_joint_angles = joint_angles
if self._trajectory_valid:
#If the specified trajectory is too far away from current joint angles, abort it
trajectory_angles = self._get_trajectory_joint_angles(
self._trajectory_t)
#TODO: improve trajectory error tracking
if (np.any(
np.abs(trajectory_angles -
joint_angles) > np.deg2rad(45))):
rospy.logerr("Trajectory aborted due to tracking error: %s",
str(np.rad2deg(trajectory_angles - joint_angles)))
self._abort_trajectory()
else:
fb = FollowJointTrajectoryFeedback()
fb.header.stamp = rospy.Time.now()
fb.desired.positions = self._get_trajectory_joint_angles(
self._trajectory_max_t)
fb.desired.time_from_start = rospy.Duration(
secs=self._trajectory_t)
fb.actual = fb.desired
gh = self._trajectory_gh
gh.publish_feedback(fb)
def __init__(self, rate=100.0):
self._alive = False
self.init_success = False
self._action_name = rospy.get_name()
# Action Feedback/Result
"""
Define the joint names
"""
self._joint_names = ['linear_joint']
"""
Controller parameters from arguments, messages, and dynamic
reconfigure
"""
self._trajectory_control_rate = rate # Hz
self._goal_time = 0.0
self._stopped_velocity = 0.0
self._goal_error = dict()
self._path_thresh = dict()
self._estop_delay = 0
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
#self._dyn = reconfig_server
self._ns = '/movo/torso_controller'
self._fjt_ns = self._ns + '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._fjt_ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._alive = True
self.estop = False
self._movo_status_sub = rospy.Subscriber("/movo/feedback/status",
Status,
self._update_movo_status)
self._js = rospy.wait_for_message("/movo/linear_actuator/joint_states",
JointState)
self.pos_targets = self._get_current_position(self._joint_names)
self._sub = rospy.Subscriber("/movo/linear_actuator/joint_states",
JointState,
self._update_movo_joint_states)
self._cmd_pub = rospy.Publisher("/movo/linear_actuator_cmd",
LinearActuatorCmd,
queue_size=10)
self._server.start()
self.init_success = True
def __init__(self, arm, serial_number, rate=100.0):
self._alive = False
self.init_success = True
self._action_name = rospy.get_name()
self._name = arm
# Action Feedback/Result
"""
Define the joint names
"""
self._joint_names = ['%s_shoulder_pan_joint'%arm,
'%s_shoulder_lift_joint'%arm,
'%s_elbow_joint'%arm,
'%s_wrist_1_joint'%arm,
'%s_wrist_2_joint'%arm,
'%s_wrist_3_joint'%arm]
"""
Controller parameters from arguments, messages, and dynamic
reconfigure
"""
self._trajectory_control_rate = rate # Hz
self._goal_time = 0.0
self._stopped_velocity = 0.0
self._goal_error = dict()
self._path_thresh = dict()
self._traj_smoother = TrajectorySmoother(rospy.get_name(),arm)
self._ctl = SIArmController(self._joint_names,arm,serial_number)
if not self._ctl.init_success:
rospy.logerr("Failed to initialize controller, make sure the serial number exists")
self.clean_shutdown()
self.init_success = False
return
self._fdbk = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
#self._dyn = reconfig_server
self._ns = '/vector/%s_arm'%arm
self._fjt_ns = self._ns + '/follow_joint_trajectory'
self._server = actionlib.SimpleActionServer(
self._fjt_ns,
FollowJointTrajectoryAction,
execute_cb=self._on_trajectory_action,
auto_start=False)
self._alive = True
self._server.start()
def initialize(self):
ns = self.controller_namespace + '/joint_trajectory_action_node/constraints'
# rospy.get_param will try to fetch the information from the parameter server,
# but if the parameter is not available, it will use the default value.
# let motors roll for specified amount of time
self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0)
self.goal_constraints = []
self.trajectory_constraints = []
# The controller will look for goals / trajectory contrains on the parameter server
# The values should be set on top of the controller namespace + '/joint_trajectory_action_node/constraints'
for joint in self.joint_names:
self.goal_constraints.append(
rospy.get_param(ns + '/' + joint + '/goal', 0.1)) #0.04
self.trajectory_constraints.append(
rospy.get_param(ns + '/' + joint + '/trajectory', 1.5))
rospy.loginfo("Goal constrains: " + ",".join([
ji + "=" + str(ci)
for ji, ci in zip(self.joint_names, self.goal_constraints)
]))
rospy.loginfo("Trajectory constrains: " + ",".join([
ji + "=" + str(ci)
for ji, ci in zip(self.joint_names, self.trajectory_constraints)
]))
# Message containing current state for all controlled joints
# control_msgs/FollowJointTrajectoryFeedback
self.feedback = FollowJointTrajectoryFeedback()
self.feedback.joint_names = self.joint_names
self.feedback.desired.positions = [0.0] * self.num_joints
self.feedback.desired.velocities = [0.0] * self.num_joints
self.feedback.desired.accelerations = [0.0] * self.num_joints
self.feedback.actual.positions = [0.0] * self.num_joints
self.feedback.actual.velocities = [0.0] * self.num_joints
self.feedback.error.positions = [0.0] * self.num_joints
self.feedback.error.velocities = [0.0] * self.num_joints
self.running = dict()
return 0
def send_goal(self, goal):
# send goals to all controllers
for controller in self.controllers:
controller.send_goal(goal)
# publish feedback until they are done
done_goals = [False] * len(self.controllers)
while not rospy.is_shutdown() and not all(done_goals):
# construct feedback message
curr_fed = FollowJointTrajectoryFeedback()
for cidx, controller in enumerate(self.controllers):
if controller.is_goal_done():
done_goals[cidx] = True
# add to feedback message this controller stuff
fed = controller.get_feedback()
# curr_fed add stuff
# compose global result and publish
for controller in self.controllers:
res = controller.get_result()
def __init__(self, name):
'''
\brief Constructor for class moveit_action
\param[in] name Name of subscribed topic.
'''
# Action variables
## Action service result
self.ac_result = FollowJointTrajectoryResult()
## Action service feedback
self.ac_feedback = FollowJointTrajectoryFeedback()
# Action Initialization
## Topic name
self.action_name = name
## Action server
self.action_server = actionlib.SimpleActionServer(
self.action_name,
FollowJointTrajectoryAction,
execute_cb=self.action_callback,
auto_start=False)
self.action_server.start()
def __init__(self, name):
self._feedback = FollowJointTrajectoryFeedback()
self._result = FollowJointTrajectoryResult()
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
FollowJointTrajectoryAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
# Getting publishers for all joints
self._poppy = rospy.get_param("/poppy")
self._joints_pub = {}
for k, d in self._poppy.iteritems():
if k != 'joint_state_controller': #just ignore this one
self._joints_pub[k.replace('_position_controller',
'')] = rospy.Publisher(
"/poppy/" + k + "/command",
Float64)
rospy.loginfo('%s: is ready' % self._action_name)
class MoveItAction(object):
_feedback = FollowJointTrajectoryFeedback()
_result = FollowJointTrajectoryResult()
def __init__(self, name):
self.publisher = rospy.Publisher("joint_path_command", JointTrajectory)
self._action_name = name
self._as = actionlib.SimpleActionServer(self._action_name,
FollowJointTrajectoryAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
def execute_cb(self, goal):
self.publisher.publish(goal.trajectory)
try:
rospy.sleep(goal.trajectory.points[-1].time_from_start)
self._result.error_code = 0
except:
self._result.error_code = self._result.OLD_HEADER_TIMESTAMP
self._as.set_succeeded(self._result)
def do_trajectory(goal):
joint_names = goal.trajectory.joint_names
trajectory_points = goal.trajectory.points
print("GOAL TRAJECTORY:")
print(goal.trajectory)
# publish normally???
left_arm_pub = rospy.Publisher('r2d2_left_arm_controller/command',
JointTrajectory,
queue_size=10)
left_arm_pub.publish(goal.trajectory)
# print("Published")
start_time = time.time()
print(time.time() - start_time)
while (time.time() - start_time < 10):
print(time.time() - start_time)
feedback = FollowJointTrajectoryFeedback()
feedback.desired = JointTrajectoryPoint()
feedback.desired = goal.trajectory.points[0]
# get actual positions from joint state or robot publisher ...
feedback.actual = JointTrajectoryPoint()
feedback.actual.positions = [0, 0, 0]
feedback.error = JointTrajectoryPoint()
feedback.error.positions = numpy.subtract(feedback.desired.positions,
feedback.actual.positions)
server.publish_feedback(feedback)
time.sleep(1.0)
result = FollowJointTrajectoryResult()
server.set_succeeded(result, "Trajectory completed successfully")
print(result.SUCCESSFUL)
class JointContoller():
_feedback = FollowJointTrajectoryFeedback()
_result = FollowJointTrajectoryResult()
def __init__(self):
self.name = rospy.get_name()
self.publisher = rospy.Publisher('joint_states',
JointState,
queue_size=10)
# Set up action client
self.action_server = actionlib.SimpleActionServer(
'%s/follow_joint_trajectory' % self.name,
FollowJointTrajectoryAction, self.do_action_callback, False)
self.action_server.start()
self.rate = rospy.Rate(10) # 10hz
self.joint_state = JointState()
self.joint_state.header = Header()
self.joint_state.name = ['hip', 'shoulder', 'elbow', 'wrist']
self.joint_state.position = [
1.57, 1.57, 1.57, 1.57
] # will be updated in home() before publication in the loop
self.joint_state.velocity = []
self.joint_state.effort = []
# Initialise the PCA9685 using the default address (0x40) and the bus
self.pwm = Adafruit_PWM_Servo_Driver.PWM(address=0x40, busnum=1)
self.pwm.setPWMFreq(cal.PWM_FREQUENCY)
self.home()
def do_action_callback(self, goal):
print 'Received goal %d' % len(goal.trajectory.points[0].positions)
timePassed = rospy.Duration(0)
self.joint_state.name = goal.trajectory.joint_names
for point in goal.trajectory.points:
self.joint_state.header.stamp = rospy.Time.now()
self.move_arm(goal.trajectory.joint_names, point.positions)
self.publisher.publish(self.joint_state)
rospy.sleep(point.time_from_start - timePassed)
timePassed = point.time_from_start
print('step')
self._result.error_code = 0
self.action_server.set_succeeded(self._result)
print('move complete')
def get_joint_goal(self, joint_name, joint_list, goal_list):
if joint_name in joint_list:
index = joint_list.index(joint_name)
return goal_list[index]
print 'ERROR!! Could not find %s goal' % joint_name
def set_joint_state(self, joint_name, pulse):
index = self.joint_state.name.index(joint_name)
print '%s at %d is %d (%0.2f)' % (joint_name, index, pulse,
math.radians(pulse))
self.joint_state.position[index] = math.radians(pulse)
def move_arm(self, names, goal):
base = self.get_joint_goal('hip', names, goal)
shoulder = self.get_joint_goal('shoulder', names, goal)
elbow = self.get_joint_goal('elbow', names, goal)
claw = self.get_joint_goal('wrist', names, goal)
self.setJointAngles(base, shoulder, elbow, claw)
def check_min_max(self, minVal, maxVal, test):
if (test < minVal):
test = minVal
print "min %d" % (minVal)
if (test > maxVal):
test = maxVal
print "min %d" % (maxVal)
return test
# Helper function to make setting a servo pulse width simpler.
def set_servo_pulse(self, channel, pulse):
pwm_val = 150 + int(
pulse * 450.0 / 180.0) # magic incantation to make the timing work
self.pwm.setPWM(channel, 0, pwm_val)
def b(self, pulse):
pulse = self.check_min_max(cal.BASE_MIN_PWM, cal.BASE_MAX_PWM, pulse)
self.set_joint_state('hip', pulse)
self.set_servo_pulse(cal.BASE_SERVO_CHANNEL, pulse)
def s(self, pulse):
pulse = self.check_min_max(cal.SHOULDER_MIN_PWM, cal.SHOULDER_MAX_PWM,
pulse)
self.set_joint_state('shoulder', pulse)
self.set_servo_pulse(cal.SHOULDER_SERVO_CHANNEL, pulse)
def e(self, pulse):
pulse = self.check_min_max(cal.ELBOW_MIN_PWM, cal.ELBOW_MAX_PWM, pulse)
self.set_joint_state('elbow', pulse)
self.set_servo_pulse(cal.ELBOW_SERVO_CHANNEL, pulse)
def c(self, pulse):
pulse = self.check_min_max(cal.CLAW_MIN_PWM, cal.CLAW_MAX_PWM, pulse)
self.set_joint_state('wrist', pulse)
self.set_servo_pulse(cal.CLAW_SERVO_CHANNEL, pulse)
def setJointAngles(self, base, shoulder, elbow, claw):
self.b(int(math.degrees(base)))
self.s(int(math.degrees(shoulder)))
self.e(int(math.degrees(elbow)))
self.c(int(math.degrees(claw)))
def home(self):
self.b(cal.BASE_HOME_PWM)
self.s(cal.SHOULDER_HOME_PWM)
self.e(cal.ELBOW_HOME_PWM)
self.c(cal.CLAW_HOME_PWM)
def off(self):
self.pwm.setPWM(cal.BASE_SERVO_CHANNEL, 0, 0)
self.pwm.setPWM(cal.SHOULDER_SERVO_CHANNEL, 0, 0)
self.pwm.setPWM(cal.ELBOW_SERVO_CHANNEL, 0, 0)
self.pwm.setPWM(cal.CLAW_SERVO_CHANNEL, 0, 0)
def loop(self):
# publish states regularly though this is also done on commands
while not rospy.is_shutdown():
self.joint_state.header.stamp = rospy.Time.now()
self.publisher.publish(self.joint_state)
self.rate.sleep()
self.off()