def run(self, joint_trajectory):
cmd = redhawk_cmd()
# Force apply velocity and acceleration limits
cmd.max_vel = max(MAX_VEL)
cmd.max_accel = MAX_ACCEL
cmd.arm_enable = False
self.pub.publish(cmd)
prev_time = 0
for i in range(len(joint_trajectory.points)):
# Wait until timestamp for next command
end_time = joint_trajectory.points[i].time_from_start.to_sec()
time.sleep(end_time - prev_time + 0.5)
prev_time = end_time
# Perpare and send command
cmd.cmd_type = cmd.CMD_POSITION
cmd.arm_enable = True
cmd.joint_1 = joint_trajectory.points[i].positions[
0] / math.pi * 180.0
cmd.joint_2 = joint_trajectory.points[i].positions[
1] / math.pi * 180.0
cmd.joint_3 = joint_trajectory.points[i].positions[
2] / math.pi * 180.0
cmd.joint_4 = joint_trajectory.points[i].positions[
3] / math.pi * 180.0
cmd.joint_5 = joint_trajectory.points[i].positions[
4] / math.pi * 180.0
cmd.joint_6 = joint_trajectory.points[i].positions[
5] / math.pi * 180.0
rospy.loginfo("[{}s] publishing command".format(prev_time))
self.pub.publish(cmd)
def set_velocities(self, vel):
cmd = redhawk_cmd()
cmd.cmd_type = cmd.CMD_VELOCITY
cmd.max_vel = max(MAX_VEL)
cmd.max_accel = MAX_ACCEL
cmd.arm_enable = True
cmd.joint_1 = vel[0]
cmd.joint_2 = vel[1]
cmd.joint_3 = vel[2]
cmd.joint_4 = vel[3]
cmd.joint_5 = vel[4]
cmd.joint_6 = vel[5]
self.pub.publish(cmd)
def stop(self):
cmd = redhawk_cmd()
cmd.cmd_type = cmd.CMD_VELOCITY
cmd.max_vel = max(MAX_VEL)
cmd.max_accel = MAX_ACCEL
cmd.arm_enable = True
cmd.joint_1 = 0
cmd.joint_2 = 0
cmd.joint_3 = 0
cmd.joint_4 = 0
cmd.joint_5 = 0
cmd.joint_6 = 0
self.pub.publish(cmd)
rospy.loginfo("STOP")
def telem_purge(self, telem_purge_duration):
telem_purge_start = time.time()
cmd = redhawk_cmd()
cmd.max_vel = max(MAX_VEL)
cmd.max_accel = MAX_ACCEL
cmd.arm_enable = True
cmd.cmd_type = cmd.CMD_VELOCITY
cmd.joint_1 = 0
cmd.joint_2 = 0
cmd.joint_3 = 0
cmd.joint_4 = 0
cmd.joint_5 = 0
cmd.joint_6 = 0
while not rospy.is_shutdown():
self.pub.publish(cmd)
self.r.sleep()
if time.time() > telem_purge_start + telem_purge_duration:
# Done.
break
def set_pose(self, angles, wait=True):
cmd = redhawk_cmd()
cmd.cmd_type = cmd.CMD_POSITION
cmd.max_vel = max(MAX_VEL)
cmd.max_accel = MAX_ACCEL
cmd.arm_enable = True
cmd.joint_1 = angles[0]
cmd.joint_2 = angles[1]
cmd.joint_3 = angles[2]
cmd.joint_4 = angles[3]
cmd.joint_5 = angles[4]
cmd.joint_6 = angles[5]
self.pub.publish(cmd)
if wait:
while not rospy.is_shutdown():
self.r.sleep()
self.pub.publish(cmd)
if self.check_pose(angles):
break
def run2(self, joint_trajectory):
joint_tolerance = rospy.get_param(
"/redhawk_controller/joint_tolerance", POSE_TOLERANCE_2)
rospy.loginfo(
"Using a joint position goal tolerance of {} degrees".format(
joint_tolerance))
cmd = redhawk_cmd()
# Force apply velocity and acceleration limits
cmd.max_vel = max(MAX_VEL)
cmd.max_accel = MAX_ACCEL
cmd.arm_enable = True
cmd.cmd_type = cmd.CMD_VELOCITY
cmd.joint_1 = 0
cmd.joint_2 = 0
cmd.joint_3 = 0
cmd.joint_4 = 0
cmd.joint_5 = 0
cmd.joint_6 = 0
# TODO: is it enough to do this in the constructor?
# Get telemetry going. This is really needed.
self.telem_purge(0.1)
start_timestamp = rospy.get_time()
prev_lateness = 0
overall_lateness = 0
try:
for i in range(len(joint_trajectory.points)):
# Calculate velocities
targets = [0, 0, 0, 0, 0, 0]
velocities = [0, 0, 0, 0, 0, 0]
target_timestamp = joint_trajectory.points[
i].time_from_start.to_sec()
point_start_timestamp = rospy.get_time() - start_timestamp
#desired_command_duration = target_timestamp - prev_lateness - point_start_timestamp
#desired_command_duration = target_timestamp - point_start_timestamp
#desired_command_duration = target_timestamp - point_start_timestamp + overall_lateness
if i > 0:
desired_command_duration = joint_trajectory.points[i].time_from_start.to_sec() \
- joint_trajectory.points[i-1].time_from_start.to_sec()
if desired_command_duration < MIN_COMMAND_DURATION:
rospy.logwarn(
"Desired command duration ({}) is very low. Clipping it to {}"
.format(desired_command_duration,
MIN_COMMAND_DURATION))
desired_command_duration = MIN_COMMAND_DURATION
else:
desired_command_duration = 0
if joint_trajectory.points[i].time_from_start.to_sec(
) != 0:
rospy.logerror(
"First point in JointTrajectory should be the current starting position!"
)
raise RuntimeError("Invalid JointTrajectory")
rospy.loginfo("desired command duration is {:.3f} s".format(
desired_command_duration))
telem = self._telem()
if telem is None:
rospy.logfatal("telemetry invalid, aborting!")
self.stop()
return False
# Joint 4 and 6 support continuous rotation
for joint, is_continuous in enumerate(
[False, False, False, True, False, True]):
target_deg = joint_trajectory.points[i].positions[
joint] / math.pi * 180.0
targets[joint] = self._reduce_target_deg(
target_deg, telem[joint])
if is_continuous:
if abs(targets[joint] - telem[joint]) >= 180:
joint_offset_deg = -math.copysign(
(360 - abs(targets[joint] - telem[joint])),
targets[joint] - telem[joint])
else:
joint_offset_deg = (targets[joint] - telem[joint])
else:
joint_offset_deg = (targets[joint] - telem[joint])
#rospy.loginfo("joint offset is {:.2f} deg".format(joint_offset_deg))
if desired_command_duration < VERY_SMALL_DURATION:
vel_raw = 0
rospy.loginfo(
"command duration is very small ({:.1f} ms)".
format(desired_command_duration * 1000))
rospy.loginfo("setting velocity to 0")
else:
vel_raw = joint_offset_deg / desired_command_duration
if desired_command_duration < -VERY_SMALL_DURATION:
vel_raw = 0
rospy.logerr("command duration is negative!!!")
#if desired_command_duration < SMALL_DURATION:
# if joint_offset_deg > SMALL_ANGLE:
# vel_raw = 0
# print("[critical warning] command duration too low; calculated velocity is probably wrong. Setting it to 0")
#vel_raw *= VEL_MULT
vel_clipped = max(min(vel_raw, MAX_VEL[joint]),
-MAX_VEL[joint])
if vel_clipped != vel_raw:
rospy.logerr("velocity clipped")
vel_raw = vel_clipped
self.stop()
raise RuntimeError("Velocity clipped")
velocities[joint] = vel_raw
# Prepare command
cmd.arm_enable = True
cmd.cmd_type = cmd.CMD_VELOCITY
goal_reached = [False, False, False, False, False, False]
#rospy.loginfo("TARGET: {:.6f} {:.6f} {:.6f} {:.6f} {:.6f} {:.6f}".format(*targets))
#rospy.loginfo("TELEMETRY: {:.6f} {:.6f} {:.6f} {:.6f} {:.6f} {:.6f}".format(*telem))
#rospy.loginfo("VELOCITIES: {:.6f} {:.6f} {:.6f} {:.6f} {:.6f} {:.6f}".format(*velocities))
while not rospy.is_shutdown():
telem = self._telem()
if telem is None:
rospy.logfatal("telemetry invalid, aborting!")
self.stop()
return False
# Check if goal reached
for joint in range(6):
joint_velocity = velocities[joint]
# if joint == 3 or joint == 5:
# joint_velocity = -joint_velocity
if self._check_angle(targets[joint], telem[joint],
joint_velocity, joint_tolerance):
goal_reached[joint] = True
actual_duration = rospy.get_time(
) - start_timestamp - point_start_timestamp
# Add a small (~1s) duration to avoid problems when desired_command_duration
# is very small (close to arm response latency)
if actual_duration > desired_command_duration * DURATION_TOLERANCE + TIMEOUT_OFFSET:
rospy.logerr("Arm took too long to reach goal.")
raise JointTrajectoryTimeoutException()
if all(goal_reached):
# Move on to next waypoint
prev_lateness = actual_duration - desired_command_duration
overall_lateness += max(0, prev_lateness)
rospy.loginfo(
"reached waypoint {} within {:.3f}s of target".
format(i, prev_lateness))
break
#rospy.logdebug(('{:.5f} '*6).format(*[target-current for target, current in zip(targets, telem)]))
#print(telem)
#print(goal_reached)
#print(velocities)
#print('---')
cmd.joint_1 = velocities[0]
cmd.joint_2 = velocities[1]
cmd.joint_3 = -velocities[2]
cmd.joint_4 = velocities[3]
cmd.joint_5 = -velocities[4]
cmd.joint_6 = velocities[5]
self.pub.publish(cmd)
self.r.sleep()
overall_lateness = rospy.get_time(
) - start_timestamp - joint_trajectory.points[
-1].time_from_start.to_sec()
rospy.loginfo(
"finished trajectory execution within {:.3f}s of planned time".
format(overall_lateness))
self.stop()
self.r.sleep()
return True
except JointTrajectoryTimeoutException:
rospy.logerr(
"stopped execution due to timeout trying to reach target pose."
)
self.stop()
return False
except RuntimeError as ex:
rospy.logerr("Exception during trajectory execution: " + str(ex))
self.stop()
return False
#!/usr/bin/env python2
import rospy
from redhawk_ctl.msg import redhawk_cmd
if __name__ == '__main__':
rospy.init_node('redhawk_test')
pub = rospy.Publisher('redhawk_cmd', redhawk_cmd)
r = rospy.Rate(30)
cmd = redhawk_cmd()
try:
while not rospy.is_shutdown():
cmd.cmd_type = cmd.CMD_POSITION
cmd.max_vel = 30
cmd.max_accel = 45
cmd.joint_1 = 0
cmd.joint_2 = 0
cmd.joint_3 = 0
cmd.joint_4 = 0
cmd.joint_5 = 0
cmd.joint_6 = 0
cmd.arm_enable = True
pub.publish(cmd)
r.sleep()
except KeyboardInterrupt:
print 'keyboard interrupt'
cmd.cmd_type = -1