class Robot(object):
def __init__(self, joint_names):
self._motion = MotionTrajectory()
self.joint_names = joint_names
def execute(self, trajectory):
self._motion.clear_waypoints()
self._motion.set_joint_names(self.joint_names)
for point in trajectory:
waypoint = MotionWaypoint()
waypoint.set_joint_angles(point)
self._motion.append_waypoint(waypoint)
print(self._motion.to_msg())
return self._motion.send_trajectory()
def move_to_joint_positions(self, positions):
self._motion.clear_waypoints()
self._motion.set_joint_names(positions.keys())
waypoint = MotionWaypoint()
waypoint.set_joint_angles(positions.values())
self._motion.append_waypoint(waypoint)
return self._motion.send_trajectory()
def run(self):
rate = rospy.Rate(100)
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5,
max_joint_accel=0.5,
corner_distance=0.05)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
self.pose.header = Header(stamp=rospy.Time.now(), frame_id='base')
self.pose.pose.position.x = 0.0
self.pose.pose.position.y = -0.6
self.pose.pose.position.z = 0.5
self.pose.pose.orientation.x = 0.5
self.pose.pose.orientation.y = -0.5
self.pose.pose.orientation.z = 0.5
self.pose.pose.orientation.w = 0.5
joint_angles = limb.joint_ordered_angles()
waypoint.set_cartesian_pose(self.pose, "right_hand", joint_angles)
self.waypoints.append(waypoint)
rospy.loginfo("Sending inital waypoint: %s",
self.waypoints[0].to_string())
traj.append_waypoint(self.waypoints[0].to_msg())
result = traj.send_trajectory()
if result is None:
rospy.logerr('Trajectory FAILED to send')
elif result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
traj.clear_waypoints()
l = Lights()
l_name = 'head_green_light'
initial_state = l.get_light_state(l_name)
for i in range(0, 2):
state = not initial_state
l.set_light_state(l_name, state)
rospy.sleep(0.5)
state = not state
l.set_light_state(l_name, state)
rospy.sleep(0.5)
l.set_light_state(l_name, True)
for i in range(0, 19):
self.gen_rand_waypoint()
sendCommand = True
while not rospy.is_shutdown():
traj.clear_waypoints()
for i in range(0, 19):
self.waypoints.pop(i)
self.gen_rand_waypoint()
for point in self.waypoints:
traj.append_waypoint(point.to_msg())
print(len(self.waypoints))
result = traj.send_trajectory(wait_for_result=True)
self.firstShutdown = True
def clean_shutdown():
if self.firstShutdown:
print("STOPPING TRAJECTORY")
traj.stop_trajectory()
traj.clear_waypoints()
l = Lights()
l.set_light_state('head_green_light', False)
self.firstShutdown = False
rospy.on_shutdown(clean_shutdown)
rate.sleep()
return
pose_goal.orientation.w = OR_W
pose_goal.orientation.x = OR_X
pose_goal.orientation.y = OR_Y
pose_goal.orientation.z = OR_Z
trans = [
-SCALING * trans[2] + BASE_X, -SCALING * trans[0] + BASE_Y,
SCALING * trans[1] + BASE_Z
]
pose_goal.position.x = trans[0]
pose_goal.position.y = trans[1]
pose_goal.position.z = trans[2]
poseStamped = PoseStamped()
poseStamped.pose = pose_goal
joint_angles = limb.joint_ordered_angles()
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
waypoint.set_cartesian_pose(poseStamped, 'right_hand', joint_angles)
traj.append_waypoint(waypoint.to_msg())
if (counter >= 30):
pub.publish(traj.to_msg())
traj.clear_waypoints()
counter = 0
rate.sleep()