def _update(self, event):
#update robot state
now = rospy.get_rostime()
msg = JointState()
msg.header.stamp = now
msg.header.frame_id = "From arduino webserver for braccio"
msg.name = joint_names
if self.traj and self.goal_handle:
#rospy.logwarn("UPDATE ************************")
#rospy.logwarn(self.traj)
if self.index == self.lenpoints:
msg_success = 'Trajectory execution successfully completed'
rospy.logwarn(msg_success)
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self.goal_handle.set_succeeded(result=res, text=msg_success)
self.goal_handle = None
self.index = 0
self.lenpoints = None
self.traj = None
else:
position = self.traj.points[self.index].positions
self.index += 1
self.current_positions = position
msg.position = self.current_positions
#msg.effort = [0] * 5
self.pub_joint_states.publish(msg)
def test_unhappy_flow(self):
"""
The robot is told to go charge.
It should then navigate to a position close to the charger.
call the dock-service then uses it's arm to plug itself in and say something when
finally getting some juice from the charger.
However, the docking fails first so the robot retries and then the arm fails at first,
so the robot says something about it's arm and then retries
"""
self.human_to_robot_speech.publish('Go charge')
nav_goal = (10, 10, 0) # Just a random goal in x, y and orientation
move_base_goal = self.move_base.await_goal()
navigated_to_goal = any(
self.move_base.match_in_received_goals(
[nav_goal], key=self.move_base.goal_to_xy_theta))
already_at_goal = self.move_base.pose_bl == nav_goal
assert navigated_to_goal or already_at_goal, "Robot is not at goal for service"
self.move_base.direct_reply(MoveBaseResult())
# Using a custom reply, we can override the default reply set on the dock service temporarily
with self.dock.custom_reply():
self.dock.reply(SetStringResponse(success=False))
# the robot notices docking has failed, so it should say something about that
say_goal = self.say.await_goal()
assert 'fail' in say_goal.text
self.say.direct_reply(SayResult(success=True))
# Then retry docking, now successfully
self.dock.reply(SetStringResponse(success=True))
with self.arm.custom_reply():
self.arm.reply(
FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.
PATH_TOLERANCE_VIOLATED))
# the robot notices docking has failed, so it should say something about that
say_goal = self.say.await_goal()
assert 'fail' in say_goal.text
self.say.direct_reply(SayResult(success=True))
self.arm.reply(
FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.SUCCESSFUL))
while not self.say.match_in_received_goals(
["Ah, some juice!", "Jummy, fresh juice!"],
key=lambda req: req.text) and not rospy.is_shutdown():
rospy.sleep(0.5)
rospy.loginfo("test_unhappy_flow succeeded")
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,
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 __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 test_happy_flow(self):
"""
The robot is told to go charge.
It should then navigate to a position close to the charger,
call the dock-service then uses it's arm to plug itself in and say something when
finally getting some juice from the charger.
"""
self.human_to_robot_speech.publish('Go charge')
nav_goal = (10, 10, 0) # Just a random goal in x, y and orientation
move_base_goal = self.move_base.await_goal(
marker='start_move_to_charger')
navigated_to_goal = any(
self.move_base.match_in_received_goals(
[nav_goal], key=self.move_base.goal_to_xy_theta))
already_at_goal = self.move_base.pose_bl == nav_goal
assert navigated_to_goal or already_at_goal, "Robot is not at goal for service"
self.move_base.direct_reply(MoveBaseResult(),
marker='end_move_to_charger')
self.dock.reply(SetStringResponse(success=True),
marker='dock_into_charger')
self.arm.reply(FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.SUCCESSFUL),
marker='arm_plug')
while not self.say.match_in_received_goals(
["Ah, some juice!", "Jummy, fresh juice!"],
key=lambda req: req.text) and not rospy.is_shutdown():
rospy.sleep(0.5)
rospy.loginfo("test_happy_flow succeeded")
def as_cb(self, goal):
#result = JointTrajectoryResult()
result = FollowJointTrajectoryResult()
#self.as_server.set_preempted(result)
self.as_cb_executed = True
self.traj = goal.trajectory
print("action server callback")
self.as_server.set_succeeded(result)
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(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 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 CB(goal):
rospy.wait_for_service("aubo_driver/moveit")
Moveit_proxy = rospy.ServiceProxy("aubo_driver/moveit", Moveit)
req = MoveitRequest(goal)
res = Moveit_proxy.call(req)
result = FollowJointTrajectoryResult()
if result.error_code is 0:
server.set_succeeded(result)
else:
server.set_aborted(result)
def cb(self, data):
"""
指定時間の間に閾値以上の力が力各センサに加わったかどうかを返す。
現在の値からの変位を見ているため、開始のタイミングに注意
Args:
ntime int32: 指定時間
nforce float64: 閾値の力
Result:
success bool:
Feedback:
force float64: 現在加えられている力
"""
num = 1
N = len(data.trajectory.points)
success = FollowJointTrajectoryResult()
success.SUCCESSFUL
end_sec = data.trajectory.points[-1].time_from_start.to_sec()
start_sec = rospy.Time.now().to_sec()
hand = self.hand_state
_fl = False
hp = data.trajectory.points[0].positions[0]
if hand > 1.0 and hp <= 0.5:
self.hand_close()
elif hand < 0.1 and hp >= 0.5:
self.hand_open()
if hp >= 0.5:
_fl = True
while (rospy.Time.now().to_sec() -
start_sec) <= (end_sec + 1.0) and not rospy.is_shutdown():
hand = self.hand_state
n = num - 1
if n < 0:
n = 0
if hp <= 0.5 and _fl:
self.hand_close()
if (rospy.Time.now().to_sec() - start_sec
) >= data.trajectory.points[n].time_from_start.to_sec() - TIME:
hp = data.trajectory.points[num].positions[0]
if hand > 1.0 and hp <= 0.5:
self.hand_close()
self.hand_close()
elif hand < 0.1 and hp >= 0.5:
self.hand_open()
num += 1
if num == N:
break
# self.action_server.publish_feedback(feedback)
self.rate.sleep()
self.action_server.set_succeeded(success)
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 execute_cb(self, goal):
"""
:type goal: FollowJointTrajectoryGoal
"""
rospy.loginfo("Got a goal!")
# Just resend the goal to the real as
# while checking for cancel goals
# Create goal
g = FollowJointTrajectoryGoal()
# We ignore path_tolerance and goal_tolerance... does not make sense in
# a gripper
g.goal_time_tolerance = goal.goal_time_tolerance
jt = self.substitute_trajectory(goal.trajectory)
g.trajectory = jt
if not self._ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr("Action server for gripper not found!")
self._as.set_aborted(FollowJointTrajectoryResult())
# Send goal subscribing to feedback to republish it
self._ac.send_goal(g, feedback_cb=self._feedback_cb)
# wait for goal to finish while checking if cancel is requested
while not self._ac.wait_for_result(rospy.Duration(0.1)):
rospy.loginfo("Waiting for goal to finish...")
# Deal with goal cancelled
if self._as.is_preempt_requested():
self._ac.cancel_all_goals()
self._as.set_preempted()
return
result = self._ac.get_result()
res = FollowJointTrajectoryResult()
if result:
res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self._as.set_succeeded(res)
else:
res.error_code = result
self._as.set_aborted(res)
def execute_cb(self, goal):
"""
:type goal: FollowJointTrajectoryGoal
"""
rospy.loginfo("Got a goal!")
# Just resend the goal to the real as
# while checking for cancel goals
# Create goal
g = FollowJointTrajectoryGoal()
# We ignore path_tolerance and goal_tolerance... does not make sense in
# a gripper
g.goal_time_tolerance = goal.goal_time_tolerance
jt = self.substitute_trajectory(goal.trajectory)
g.trajectory = jt
if not self._ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr("Action server for gripper not found!")
self._as.set_aborted(FollowJointTrajectoryResult())
# Send goal subscribing to feedback to republish it
self._ac.send_goal(g, feedback_cb=self._feedback_cb)
# wait for goal to finish while checking if cancel is requested
while not self._ac.wait_for_result(rospy.Duration(0.1)):
rospy.loginfo("Waiting for goal to finish...")
# Deal with goal cancelled
if self._as.is_preempt_requested():
self._ac.cancel_all_goals()
self._as.set_preempted()
return
result = self._ac.get_result()
res = FollowJointTrajectoryResult()
if result:
res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self._as.set_succeeded(res)
else:
res.error_code = result
self._as.set_aborted(res)
def execute_cb(self, goal):
z_only = False
if len(goal.trajectory.joint_names) == 1:
goal.trajectory.joint_names.append("virtual_lifter_x_joint")
z_only = True
if not self.equalJoints(goal.trajectory.joint_names):
rospy.logerr("inaccurate joint names received")
return
print "execute" + rospy.get_name()
print "joint_names"
print goal.trajectory.joint_names
print goal.trajectory.points[-1].positions[0]
try:
srv = rospy.ServiceProxy("aero_torso_controller",
AeroTorsoController)
if z_only:
x = 0
else:
x = goal.trajectory.points[-1].positions[
goal.trajectory.joint_names.index(
"virtual_lifter_x_joint")] * 1000
z = goal.trajectory.points[-1].positions[
goal.trajectory.joint_names.index(
"virtual_lifter_z_joint")] * 1000
res = srv(x, z, "world")
result = FollowJointTrajectoryResult()
if res.status == "success":
time.sleep(res.time_sec)
result.error_code = 0
self._as.set_succeeded()
rospy.loginfo('%s: Succeeded' % self._action_name)
return
#error syori wo kaku
self._as.set_aborted()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
self._as.set_aborted()
def cb(self, data):
"""
指定時間の間に閾値以上の力が力各センサに加わったかどうかを返す。
現在の値からの変位を見ているため、開始のタイミングに注意
Args:
ntime int32: 指定時間
nforce float64: 閾値の力
Result:
success bool:
Feedback:
force float64: 現在加えられている力
"""
num = 1
N = len(data.trajectory.points)
success = FollowJointTrajectoryResult()
success.SUCCESSFUL
end_sec = data.trajectory.points[-1].time_from_start.to_sec()
start_sec = rospy.Time.now().to_sec()
now = rospy.Time(0)
joint = JointTrajectory()
joint.joint_names = [
"arm_lift_joint", "arm_flex_joint", "arm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
j = JointTrajectoryPoint()
j.positions = data.trajectory.points
j.time_from_start = data.trajectory.points[0].time_from_start - now
now = data.trajectory.points[0].time_from_start
joint.points = [j]
self.grip_act.publish(joint)
while (rospy.Time.now().to_sec() -
start_sec) <= (end_sec + 1.0) and not rospy.is_shutdown():
if (rospy.Time.now().to_sec() - start_sec
) >= data.trajectory.points[num -
1].time_from_start.to_sec() - TIME:
j = JointTrajectoryPoint()
j.positions = data.trajectory.points
j.time_from_start = data.trajectory.points[
num].time_from_start - now
now = data.trajectory.points[0].time_from_start
joint.points = [j]
self.grip_act.publish(joint)
num += 1
if num == N:
break
# self.action_server.publish_feedback(feedback)
self.rate.sleep()
self.action_server.set_succeeded(success)
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, 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 __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 __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 execute_cb(self, goal):
rospy.loginfo('Receving controller trajectories...')
jt = JointTrajectory()
jt.header.stamp = rospy.Time.now()
jt.joint_names = goal.trajectory.joint_names
jt.points = []
for point in goal.trajectory.points:
jtp = JointTrajectoryPoint()
jtp.positions = point.positions
jtp.velocities = point.velocities
jtp.accelerations = point.accelerations
jtp.effort = point.effort
jtp.time_from_start = point.time_from_start
jt.points.append(jtp)
self.dx_trajectory_pub.publish(jt)
result_ = FollowJointTrajectoryResult()
self._as.set_succeeded(result_)
rospy.loginfo('Joint trajectories has been successfuly transmitted to dynamixel branch.')
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 __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 ActionServer(object):
"""Hosts the actionlib interface for the FANUC robot.
"""
__result = FollowJointTrajectoryResult()
def __init__(self, traj_topic, server_address, buffer_size=5):
assert isinstance(traj_topic, str)
assert isinstance(server_address, str)
assert isinstance(buffer_size, int)
assert buffer_size > 0
assert buffer_size < 10
self.__robot_comm_lock = threading.Lock()
self.__traj_topic = traj_topic
self.__status_monitor = FanucStatusMonitor()
self.__server_address = server_address
rospy.loginfo('Hosting trajectory action server at %s',
self.__traj_topic)
self.__trajectory_asrv = actionlib.SimpleActionServer(
self.__traj_topic,
FollowJointTrajectoryAction,
execute_cb=self.__execute_traj_callback,
auto_start=False)
self.__trajectory_asrv.start()
def __execute_traj_callback(self, goal):
rospy.loginfo('Executing trajectory')
if rospy.is_shutdown():
return
self.__robot_comm_lock.acquire()
try:
traj_runner = TrajRunner(self.__status_monitor)
traj_runner.execute_trajectory(goal)
self.__trajectory_asrv.set_succeeded()
finally:
self.__robot_comm_lock.release()
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 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)
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()
def execute_position_cb(self, goal):
is_timed_out = False
is_over_effort = False
over_effort_counter = 0
start = rospy.Time.now()
extra_time = rospy.Duration(5.0)
# print goal
for i in range(len(goal.trajectory.points)):
joint_positions = brics_actuator.msg.JointPositions()
conf = np.array(goal.trajectory.points[i].positions)
# transform from ROS to BRICS message
for j in range(len(self.joint_names)):
joint_value = brics_actuator.msg.JointValue()
joint_value.joint_uri = self.joint_names[j]
joint_value.value = self.limit_joint(self.joint_names[j], conf[j])
conf[j] = joint_value.value
joint_value.unit = self.unit
joint_positions.positions.append(joint_value)
if self.is_over_effort():
over_effort_counter += 1
rospy.logerr("%d, over effort", over_effort_counter)
if over_effort_counter > LIMIT_OVER_EFFORT:
is_over_effort = True
break
self.position_pub.publish(joint_positions)
# wait to reach the goal position
while not rospy.is_shutdown():
if self.is_over_effort():
over_effort_counter += 1
rospy.logerr("%d, over effort", over_effort_counter)
if over_effort_counter > LIMIT_OVER_EFFORT:
is_over_effort = True
break
if self.is_goal_reached(conf, self.configuration):
rospy.sleep(0.01)
break
if (rospy.Time.now() - start) > (goal.trajectory.points[i].time_from_start + extra_time):
is_timed_out = True
break
rospy.sleep(0.01)
if is_timed_out or is_over_effort:
break
result = FollowJointTrajectoryResult()
if is_over_effort:
result.error_code = FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
rospy.logerr("arm over-effort")
self.arm_up_recover()
self.position_action_server.set_preempted(result)
elif is_timed_out:
result.error_code = FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
rospy.logerr("arm timed_out")
self.arm_up_recover()
self.position_action_server.set_aborted(result)
else:
result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
self.position_action_server.set_succeeded(result)
def process_trajectory(self, traj):
self.traj_pub.publish(traj)
num_points = len(traj.points)
# @TODO
# make sure the joints in the goal are a subset of joints of the controller
if set(self.joint_names) != set(traj.joint_names):
if set(self.joint_names) >= set(traj.joint_names):
rospy.logwarn('Partial joint names')
print traj.joint_names
res = FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.SUCCESSFUL)
self.action_server.set_succeeded(result=res, text='')
return
else:
res = FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.INVALID_JOINTS)
msg = 'Incoming trajectory joints do not match the joints of the controller'
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
# make sure trajectory is not empty
if num_points == 0:
msg = 'Incoming trajectory is empty'
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
return
# correlate the joints we're commanding to the joints in the message
# map from an index of joint in the controller to an index in the trajectory
lookup = [traj.joint_names.index(joint) for joint in self.joint_names]
durations = [0.0] * num_points
# find out the duration of each segment in the trajectory
durations[0] = traj.points[0].time_from_start.to_sec()
for i in range(1, num_points):
durations[i] = (traj.points[i].time_from_start -
traj.points[i - 1].time_from_start).to_sec()
if not traj.points[0].positions:
res = FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.INVALID_GOAL)
msg = 'First point of trajectory has no positions'
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
trajectory = []
time = rospy.Time.now() + rospy.Duration(0.01)
for i in range(num_points):
seg = Segment(self.num_joints)
if traj.header.stamp == rospy.Time(0.0):
seg.start_time = (time + traj.points[i].time_from_start
).to_sec() - durations[i]
else:
seg.start_time = (
traj.header.stamp +
traj.points[i].time_from_start).to_sec() - durations[i]
seg.duration = durations[i]
# Checks that the incoming segment has the right number of elements.
if traj.points[i].velocities and len(
traj.points[i].velocities) != self.num_joints:
res = FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.INVALID_GOAL)
msg = 'Command point %d has %d elements for the velocities' % (
i, len(traj.points[i].velocities))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
if len(traj.points[i].positions) != self.num_joints:
res = FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.INVALID_GOAL)
msg = 'Command point %d has %d elements for the positions' % (
i, len(traj.points[i].positions))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
for j in range(self.num_joints):
if traj.points[i].velocities:
seg.velocities[j] = traj.points[i].velocities[lookup[j]]
if traj.points[i].positions:
seg.positions[j] = traj.points[i].positions[lookup[j]]
trajectory.append(seg)
rospy.loginfo('Trajectory start requested at %.3lf, waiting...',
traj.header.stamp.to_sec())
rate = rospy.Rate(self.update_rate)
while traj.header.stamp > time:
time = rospy.Time.now()
rate.sleep()
end_time = traj.header.stamp + rospy.Duration(sum(durations))
seg_end_times = [
rospy.Time.from_sec(trajectory[seg].start_time + durations[seg])
for seg in range(len(trajectory))
]
rospy.loginfo(
'Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf',
time.to_sec(), end_time.to_sec(), sum(durations))
self.trajectory = trajectory
traj_start_time = rospy.Time.now()
for seg in range(len(trajectory)):
rospy.logdebug(
'current segment is %d time left %f cur time %f' %
(seg, durations[seg] -
(time.to_sec() - trajectory[seg].start_time), time.to_sec()))
rospy.logdebug('goal positions are: %s' %
str(trajectory[seg].positions))
# first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it
if durations[seg] == 0:
rospy.logdebug(
'skipping segment %d with duration of 0 seconds' % seg)
continue
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
j = self.joint_names.index(joint)
start_position = self.joint_states[joint].current_pos
if seg != 0:
start_position = trajectory[seg - 1].positions[j]
desired_position = trajectory[seg].positions[j]
desired_velocity = max(
self.min_velocity,
abs(desired_position - start_position) /
durations[seg])
self.msg.desired.positions[j] = desired_position
self.msg.desired.velocities[j] = desired_velocity
# probably need a more elegant way of figuring out if we are dealing with a dual controller
if hasattr(self.joint_to_controller[joint], "master_id"):
master_id = self.joint_to_controller[joint].master_id
slave_id = self.joint_to_controller[joint].slave_id
master_pos, slave_pos = self.joint_to_controller[
joint].pos_rad_to_raw(desired_position)
spd = self.joint_to_controller[joint].spd_rad_to_raw(
desired_velocity)
vals.append((master_id, master_pos, spd))
vals.append((slave_id, slave_pos, spd))
else:
motor_id = self.joint_to_controller[joint].motor_id
pos = self.joint_to_controller[joint].pos_rad_to_raw(
desired_position)
spd = self.joint_to_controller[joint].spd_rad_to_raw(
desired_velocity)
vals.append((motor_id, pos, spd))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position_and_speed(vals)
while time < seg_end_times[seg]:
# check if new trajectory was received, if so abort current trajectory execution
# by setting the goal to the current position
if self.action_server.is_preempt_requested():
msg = 'New trajectory received. Aborting old trajectory.'
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
cur_pos = self.joint_states[joint].current_pos
motor_id = self.joint_to_controller[joint].motor_id
pos = self.joint_to_controller[
joint].pos_rad_to_raw(cur_pos)
vals.append((motor_id, pos))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position(vals)
self.action_server.set_preempted(text=msg)
rospy.logwarn(msg)
return
rate.sleep()
time = rospy.Time.now()
# Verifies trajectory constraints
for j, joint in enumerate(self.joint_names):
if self.trajectory_constraints[
j] > 0 and self.msg.error.positions[
j] > self.trajectory_constraints[j]:
msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \
(joint, seg, self.msg.error.positions[j], self.trajectory_constraints[j])
rospy.logwarn(msg)
# Define result msg
res = FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.
PATH_TOLERANCE_VIOLATED)
self.action_server.set_aborted(result=res, text=msg)
return
# let motors roll for specified amount of time
rospy.sleep(self.goal_time_constraint)
for i, j in enumerate(self.joint_names):
rospy.logdebug(
'desired pos was %f, actual pos is %f, error is %f' %
(trajectory[-1].positions[i], self.joint_states[j].current_pos,
self.joint_states[j].current_pos -
trajectory[-1].positions[i]))
# Checks that we have ended inside the goal constraints
for (joint, pos_error, pos_constraint) in zip(self.joint_names,
self.msg.error.positions,
self.goal_constraints):
if pos_constraint > 0 and abs(pos_error) > pos_constraint:
msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \
(joint, pos_error, pos_constraint)
rospy.logwarn(msg)
# Define result msg
res = FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.SUCCESSFUL)
self.action_server.set_aborted(result=res, text=msg)
break
else:
msg = 'Trajectory execution successfully completed'
rospy.loginfo(msg)
# Define result msg
res = FollowJointTrajectoryResult(
error_code=FollowJointTrajectoryResult.SUCCESSFUL)
self.action_server.set_succeeded(result=res, text=msg)
def process_trajectory(self, traj):
# Optionally convert from URDF convention to Dynamixel Convention
if self.has_converter:
traj = self.converter.convert_trajectory(traj)
num_points = len(traj.points)
# make sure the joints in the goal match the joints of the controller
if set(self.joint_names) != set(traj.joint_names):
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_JOINTS
msg = 'Incoming trajectory joints do not match the joints of the controller:\n'
msg = msg + "Expected: " + str(self.joint_names) + "\nReceived: "+ str(traj.joint_names)
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
# make sure trajectory is not empty
if num_points == 0:
msg = 'Incoming trajectory is empty'
rospy.logerr(msg)
self.action_server.set_aborted(text=msg)
return
# correlate the joints we're commanding to the joints in the message
# map from an index of joint in the controller to an index in the trajectory
lookup = [traj.joint_names.index(joint) for joint in self.joint_names]
durations = [0.0] * num_points
# find out the duration of each segment in the trajectory
durations[0] = traj.points[0].time_from_start.to_sec()
for i in range(1, num_points):
durations[i] = (traj.points[i].time_from_start - traj.points[i - 1].time_from_start).to_sec()
if not traj.points[0].positions:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'First point of trajectory has no positions'
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
trajectory = []
time = rospy.Time.now() + rospy.Duration(0.01)
for i in range(num_points):
seg = Segment(self.num_joints)
if traj.header.stamp == rospy.Time(0.0):
seg.start_time = (time + traj.points[i].time_from_start).to_sec() - durations[i]
else:
seg.start_time = (traj.header.stamp + traj.points[i].time_from_start).to_sec() - durations[i]
seg.duration = durations[i]
# Checks that the incoming segment has the right number of elements.
if traj.points[i].velocities and len(traj.points[i].velocities) != self.num_joints:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'Command point %d has %d elements for the velocities' % (i, len(traj.points[i].velocities))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
if len(traj.points[i].positions) != self.num_joints:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.INVALID_GOAL
msg = 'Command point %d has %d elements for the positions' % (i, len(traj.points[i].positions))
rospy.logerr(msg)
self.action_server.set_aborted(result=res, text=msg)
return
for j in range(self.num_joints):
if traj.points[i].velocities:
seg.velocities[j] = traj.points[i].velocities[lookup[j]]
if traj.points[i].positions:
seg.positions[j] = traj.points[i].positions[lookup[j]]
trajectory.append(seg)
rospy.loginfo('Trajectory start requested at %.3lf, waiting...', traj.header.stamp.to_sec())
rate = rospy.Rate(self.update_rate)
while traj.header.stamp > time:
time = rospy.Time.now()
rate.sleep()
end_time = traj.header.stamp + rospy.Duration(sum(durations))
seg_end_times = [rospy.Time.from_sec(trajectory[seg].start_time + durations[seg]) for seg in range(len(trajectory))]
rospy.loginfo('Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf', time.to_sec(), end_time.to_sec(), sum(durations))
self.trajectory = trajectory
traj_start_time = rospy.Time.now()
for seg in range(len(trajectory)):
rospy.logdebug('current segment is %d time left %f cur time %f' % (seg, durations[seg] - (time.to_sec() - trajectory[seg].start_time), time.to_sec()))
rospy.logdebug('goal positions are: %s' % str(trajectory[seg].positions))
# first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it
if durations[seg] == 0:
rospy.logdebug('skipping segment %d with duration of 0 seconds' % seg)
continue
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
j = self.joint_names.index(joint)
start_position = self.joint_states[joint].current_pos
if seg != 0: start_position = trajectory[seg - 1].positions[j]
desired_position = trajectory[seg].positions[j]
desired_velocity = max(self.min_velocity, abs(desired_position - start_position) / durations[seg])
self.msg.desired.positions[j] = desired_position
self.msg.desired.velocities[j] = desired_velocity
# probably need a more elegant way of figuring out if we are dealing with a dual controller
if hasattr(self.joint_to_controller[joint], "master_id"):
master_id = self.joint_to_controller[joint].master_id
slave_id = self.joint_to_controller[joint].slave_id
master_pos, slave_pos = self.joint_to_controller[joint].pos_rad_to_raw(desired_position)
spd = self.joint_to_controller[joint].spd_rad_to_raw(desired_velocity)
vals.append((master_id, master_pos, spd))
vals.append((slave_id, slave_pos, spd))
else:
motor_id = self.joint_to_controller[joint].motor_id
pos = self.joint_to_controller[joint].pos_rad_to_raw(desired_position)
spd = self.joint_to_controller[joint].spd_rad_to_raw(desired_velocity)
vals.append((motor_id, pos, spd))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position_and_speed(vals)
while time < seg_end_times[seg]:
# check if new trajectory was received, if so abort current trajectory execution
# by setting the goal to the current position
if self.action_server.is_preempt_requested():
msg = 'New trajectory received. Aborting old trajectory.'
multi_packet = {}
for port, joints in self.port_to_joints.items():
vals = []
for joint in joints:
cur_pos = self.joint_states[joint].current_pos
motor_id = self.joint_to_controller[joint].motor_id
pos = self.joint_to_controller[joint].pos_rad_to_raw(cur_pos)
vals.append((motor_id, pos))
multi_packet[port] = vals
for port, vals in multi_packet.items():
self.port_to_io[port].set_multi_position(vals)
self.action_server.set_preempted(text=msg)
rospy.logwarn(msg)
return
rate.sleep()
time = rospy.Time.now()
# Verifies trajectory constraints
for j, joint in enumerate(self.joint_names):
if self.trajectory_constraints[j] > 0 and self.msg.error.positions[j] > self.trajectory_constraints[j]:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED
msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \
(joint, seg, self.msg.error.positions[j], self.trajectory_constraints[j])
rospy.logwarn(msg)
self.action_server.set_aborted(result=res, text=msg)
return
# let motors roll for specified amount of time
rospy.sleep(self.goal_time_constraint)
for i, j in enumerate(self.joint_names):
rospy.logdebug('desired pos was %f, actual pos is %f, error is %f' % (trajectory[-1].positions[i], self.joint_states[j].current_pos, self.joint_states[j].current_pos - trajectory[-1].positions[i]))
# Checks that we have ended inside the goal constraints
for (joint, pos_error, pos_constraint) in zip(self.joint_names, self.msg.error.positions, self.goal_constraints):
if pos_constraint > 0 and abs(pos_error) > pos_constraint:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED
msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \
(joint, pos_error, pos_constraint)
rospy.logwarn(msg)
self.action_server.set_aborted(result=res, text=msg)
break
else:
res = FollowJointTrajectoryResult()
res.error_code = FollowJointTrajectoryResult.SUCCESSFUL
msg = 'Trajectory execution successfully completed'
rospy.loginfo(msg)
self.action_server.set_succeeded(result=res, text=msg)