def __init__(self):
print("Getting robot state... ")
self.rs = RobotEnable(CHECK_VERSION)
init_state = self.rs.state().enabled
self.rs.enable()
self.pose = PoseStamped()
self.prevX = 0.65
self.prevY = 0.0
self.waypoints = []
ap = argparse.ArgumentParser()
ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough")
ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition (true/false)")
ap.add_argument("-b", "--box", type=bool, help="Test box edges at progam startup (true/false)")
ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position")
self.args = vars(ap.parse_args())
if self.args["condition"] is None:
self.args["condition"] = "calm"
if not self.args["randomize"]:
self.args["randomize"] = False
if not self.args["box"]:
self.args["box"] = False
outargs = dict(zip(self.args.keys(), self.args.values()))
print('')
print('')
print(outargs)
print('')
print('--------------------------------------------------------------------------------------------')
print('')
def __init__(self):
self._done = False
self._head = Head()
self._robot_state = RobotEnable(CHECK_VERSION)
self._init_state = self._robot_state.state().enabled
rospy.Subscriber("robot/head/head_state", HeadState, self.straighten)
def __init__(self):
"""
'Wobbles' both arms by commanding joint velocities sinusoidally.
"""
print("Initializing node... ")
rospy.init_node("rsdk_dish_rack")
rospy.on_shutdown(self.clean_shutdown)
self._right_arm = limb.Limb("right")
self._right_joint_names = self._right_arm.joint_names()
# control parameters
self._rate = 20.0 # Hz
self._missed_cmds = 3
self.control_rate = rospy.Rate(self._rate)
print("Getting robot state... ")
self._rs = RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self._init_joint_angles = [
self._right_arm.joint_angle(joint_name)
for joint_name in self._right_joint_names
]
rospy.set_param('named_poses/right/poses/neutral',
self._init_joint_angles)
self._right_arm.set_command_timeout(
(1.0 / self._rate) * self._missed_cmds)
def __init__(self):
self._done = False
self._head = Head()
self._limb=Limb()
self._robot_state = RobotEnable(CHECK_VERSION)
self._init_state = self._robot_state.state().enabled
self._robot_state.enable()
self._display = HeadDisplay()
face_forward_service = rospy.Service('head/face_forward', FaceForward, self.face_forward)
rotate_head_service= rospy.Service('head/pan_to_angle', PanToAngle, self.pan_to_angle)
def __init__(self):
print("Getting robot state... ")
if ACCESS_ROBOT:
self.rs = RobotEnable(CHECK_VERSION)
self.rs.enable()
self.STOP = False
self.pose = PoseStamped()
self.prevX = 0.65
self.prevY = 0.0
self.l = Lights()
self.l_name = 'head_green_light'
self.r_name = 'head_red_light'
ap = argparse.ArgumentParser()
ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough")
ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition (true/false)")
ap.add_argument("-b", "--box", action='store_true', help="Test box edges at progam startup (true/false)")
ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position")
self.args = vars(ap.parse_args())
if self.args["condition"] is None:
self.args["condition"] = "calm"
if not self.args["randomize"]:
self.args["randomize"] = False
if not self.args["box"]:
self.args["box"] = False
outargs = dict(zip(self.args.keys(), self.args.values()))
print('')
print('')
print(outargs)
print('')
print('')
print('')
print('')
self.parser = InputData()
# ENABLE RED LIGHT WHILE WAITING FOR USER INPUT
self.l.set_light_state(self.r_name, on=True)
try:
input(" ----- ROBOT ENABLED, PLEASE PRESS 'ENTER' TO CONTINUE ----- ")
except Exception:
print("")
# ENABLE GREEN LIGHT WHEN PROGRAM IS RUNNING
self.l.set_light_state(self.r_name, on=False)
self.l.set_light_state(self.l_name, on=True)
def __init__(self):
print("Getting robot state... ")
if ACCESS_ROBOT:
self.rs = RobotEnable(CHECK_VERSION)
self.rs.enable()
self.STOP = False
self.pose = PoseStamped()
self.prevX = 0.65
self.prevY = 0.0
ap = argparse.ArgumentParser()
ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough")
ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition")
ap.add_argument("-b", "--box", action='store_true', help="Test box edges at progam startup")
ap.add_argument("-l", "--line", action='store_true', help="Move in straight line to test Doppler effect")
ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position")
self.args = vars(ap.parse_args())
if self.args["condition"] is None:
self.args["condition"] = "calm"
if not self.args["randomize"]:
self.args["randomize"] = False
if not self.args["box"]:
self.args["box"] = False
if not self.args["line"]:
self.args["line"] = False
outargs = dict(zip(self.args.keys(), self.args.values()))
print('')
print('')
print(outargs)
print('')
print('')
print('')
print('')
self.parser = InputData()
try:
input(" ----- ROBOT ENABLED, PLEASE PRESS 'ENTER' TO CONTINUE ----- ")
except Exception:
print("")
class Sawyer_Head:
def __init__(self):
self._done = False
self._head = Head()
self._robot_state = RobotEnable(CHECK_VERSION)
self._init_state = self._robot_state.state().enabled
rospy.Subscriber("robot/head/head_state", HeadState, self.straighten)
def straighten(self,headState):
try:
self._head.set_pan(math.pi/2,speed=1.0,timeout=1.0,active_cancellation=True)
except OSError, e:
pass
class PositionControl:
def __init__(self):
print("Getting robot state... ")
self.rs = RobotEnable(CHECK_VERSION)
init_state = self.rs.state().enabled
self.rs.enable()
self.pose = PoseStamped()
self.prevX = 0.65
self.prevY = 0.0
self.waypoints = []
ap = argparse.ArgumentParser()
ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough")
ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition (true/false)")
ap.add_argument("-b", "--box", type=bool, help="Test box edges at progam startup (true/false)")
ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position")
self.args = vars(ap.parse_args())
if self.args["condition"] is None:
self.args["condition"] = "calm"
if not self.args["randomize"]:
self.args["randomize"] = False
if not self.args["box"]:
self.args["box"] = False
outargs = dict(zip(self.args.keys(), self.args.values()))
print('')
print('')
print(outargs)
print('')
print('--------------------------------------------------------------------------------------------')
print('')
def gen_path(self):
limb = Limb()
for j in range (0, 1):
if self.args["randomize"]:
aX = random.uniform(0.0,0.17)
aY = random.uniform(0.0,0.17)
aZ = random.uniform(0.0,0.17)
bX = 0#random.uniform(0.01,0.011)
bY = 0#random.uniform(0.01,0.011)
bZ = 0#random.uniform(0.01,0.011)
gX = random.randint(0,1)
gY = random.randint(0,1)
gZ = random.randint(0,1)
damper = 35
print(aX, aY, aZ, bX, bY, bZ, gX, gY, gZ, damper)
else:
if self.args["condition"] == "rough":
aX = 0.1120
aY = 0.1230
aZ = 0.1568
bX = 0.0#0.0107
bY = 0.0#0.0108
bZ = 0.0#0.0120
self. gX = 0
gY = 0
gZ = 0
damper = 20
class Sawyer_Head:
def get_neutral_base_joint_angle(self):
angle=rospy.get_param("named_poses/right/poses/shipping")[1]
return angle
def __init__(self):
self._done = False
self._head = Head()
self._limb=Limb()
self._robot_state = RobotEnable(CHECK_VERSION)
self._init_state = self._robot_state.state().enabled
self._robot_state.enable()
self._display = HeadDisplay()
face_forward_service = rospy.Service('head/face_forward', FaceForward, self.face_forward)
rotate_head_service= rospy.Service('head/pan_to_angle', PanToAngle, self.pan_to_angle)
# arrow_key_control_servie = rospy.Service('head/arrow_key_control', PanToAngle, self.turn_with_arrow_keys)
# rospy.Subscriber("robot/head/head_state", HeadState, self.straighten)
# def straighten(self,headState):
# try:
# self._head.set_pan(math.pi/2,speed=1.0,timeout=1.0,active_cancellation=True)
# except OSError, e:
# print(e)
def get_base_joint_angle(self):
base_joint_name=self._limb.joint_names()[0]
return self._limb.joint_angle(base_joint_name)
def set_angle(self,angle):
'''
Sets head to the desired angle
0 - left
pi/2 - forward
pi - right
'''
self._head.set_pan(angle)
def pan_to_angle(self, req):
try:
angle=req.theta
self._head.set_pan(angle)
return PanToAngleResponse(True)
except:
return PanToAngleResponse(False)
def face_forward(self, object):
'''
Sets head facing forward regardless of body position
'''
try:
base_angle= self.get_base_joint_angle()
head_angle= self._head.pan()
angle=-1*base_angle+ math.pi/2
self.set_angle(angle)
return FaceForwardResponse(True)
except:
return FaceForwardResponse(False)
def turn_head(self, req):
try:
head_angle=self._head.pan()
if req.left:
self.set_angle(head_angle+math.radians(3))
else:
self.set_angle(head_angle-math.radians(3))
except:
rospy.logwarn("can't turn head any further")
def display(self, face):
'''
display a face on sawyer
'''
pass
class RealEnv(Env):
timestep = 0
ep_len = 128
def step(self, action):
cmd = dict([(joint, action)
for joint, action in zip(self._right_joint_names, action)])
self._right_arm.set_joint_velocities(cmd)
self.control_rate.sleep()
self.timestep += 1
ob = self._get_obs()
done = (self.timestep == self.ep_len)
return ob, 0, done, dict()
def _get_obs(self):
return [
self._right_arm.joint_angle(joint_name)
for joint_name in self._right_joint_names
]
def reset(self):
stop_action = [0.] * len(self._right_joint_names)
cmd = dict([
(joint, action)
for joint, action in zip(self._right_joint_names, stop_action)
])
self._right_arm.set_joint_velocities(cmd)
self.timestep = 0
input("Move the arm into open space and press Enter to continue.")
# SETS TO A NEUTRAL POSITION. Remove from dish rack first.
self.set_neutral()
return self._get_obs()
def render(self, mode='human'):
pass
def __init__(self):
"""
'Wobbles' both arms by commanding joint velocities sinusoidally.
"""
print("Initializing node... ")
rospy.init_node("rsdk_dish_rack")
rospy.on_shutdown(self.clean_shutdown)
self._right_arm = limb.Limb("right")
self._right_joint_names = self._right_arm.joint_names()
# control parameters
self._rate = 20.0 # Hz
self._missed_cmds = 3
self.control_rate = rospy.Rate(self._rate)
print("Getting robot state... ")
self._rs = RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self._init_joint_angles = [
self._right_arm.joint_angle(joint_name)
for joint_name in self._right_joint_names
]
rospy.set_param('named_poses/right/poses/neutral',
self._init_joint_angles)
self._right_arm.set_command_timeout(
(1.0 / self._rate) * self._missed_cmds)
def set_neutral(self):
"""
Sets both arms back into a neutral pose.
"""
print("Moving to neutral pose...")
self._right_arm.move_to_neutral()
def clean_shutdown(self):
"""
Switches out of joint torque mode to exit cleanly
"""
print("\nExiting example...")
self._right_arm.exit_control_mode()
if not self._init_state and self._rs.state().enabled:
print("Disabling robot...")
self._rs.disable()
def make_cmd(self, joint_names, action):
return dict([(joint, action)
for joint, action in zip(joint_names, action)])
class Drone:
def __init__(self):
print("Getting robot state... ")
if ACCESS_ROBOT:
self.rs = RobotEnable(CHECK_VERSION)
self.rs.enable()
self.STOP = False
self.pose = PoseStamped()
self.prevX = 0.65
self.prevY = 0.0
ap = argparse.ArgumentParser()
ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough")
ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition")
ap.add_argument("-b", "--box", action='store_true', help="Test box edges at progam startup")
ap.add_argument("-l", "--line", action='store_true', help="Move in straight line to test Doppler effect")
ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position")
self.args = vars(ap.parse_args())
if self.args["condition"] is None:
self.args["condition"] = "calm"
if not self.args["randomize"]:
self.args["randomize"] = False
if not self.args["box"]:
self.args["box"] = False
if not self.args["line"]:
self.args["line"] = False
outargs = dict(zip(self.args.keys(), self.args.values()))
print('')
print('')
print(outargs)
print('')
print('')
print('')
print('')
self.parser = InputData()
try:
input(" ----- ROBOT ENABLED, PLEASE PRESS 'ENTER' TO CONTINUE ----- ")
except Exception:
print("")
# ENSURE THE WAYPOINTS LIST IS CLEARED AT TERMINATION OF PROGRAM
def clean_shutdown(self):
print("Stopping arm...")
try:
self.STOP = True
self.move(point_list = None)
except:
print("There may have been an error exiting")
print("Stop successful, exiting...")
return
def sim_drone(self):
M = self.parser.inputMatrix()
point_list = list()
i = 0
for waypoint in M:
#point = [ waypoint[2][0],-waypoint[2][1],-waypoint[2][2],waypoint[3][0],waypoint[3][1],waypoint[3][2] ]
point = [ waypoint[2][0],-waypoint[2][1],-waypoint[2][2],0,0,0 ]
point_list.append(point)
print(point)
if i > 150: # Each point represent 0.1 seconds of flight time in simulator
break
else:
i += 1
self.move(wait=True, point_list=point_list, MAX_LIN_ACCL=2.5)
# CONTAINS WAYPOINTS TO TRACE BOX AT START OF PROGRAM
def trace_box(self):
print("I am tracing a box")
point_list = list()
point = [0.0, 0.25, 0.0, 0.0, 0.0, 0.0]
point_list.append(point)
point = [0.0, 0.25, 0.1, 0.0, 0.0, 0.0]
point_list.append(point)
point = [0.0, -0.25, 0.1, 0.0, 0.0, 0.0]
point_list.append(point)
point = [0.0, -0.25, -0.1, 0.0, 0.0, 0.0]
point_list.append(point)
point = [0.0, 0.25, -0.1, 0.0, 0.0, 0.0]
point_list.append(point)
point = [0.0, 0.25, 0.0, 0.0, 0.0, 0.0]
point_list.append(point)
success = self.move(wait=True, point_list=point_list, MAX_LIN_SPD=0.5, MAX_LIN_ACCL=0.75)
return success
def follow_line(self):
print("I am moving in a line")
point_list = list()
point = [0.0, 0.4, 0.0, 0.0, 0.0, 0.0]
point_list.append(point)
point = [0.0, -0.4, 0.0, 0.0, 0.0, 0.0]
point_list.append(point)
success = self.move(wait=True, point_list=point_list, MAX_LIN_SPD=1.0, MAX_LIN_ACCL=20.0)
return success
def moveToNeutral(self):
print("\n --- Returning to neutral position (0.65, 0.0, 0.4, 0.0, 0.0, 0.0 ---")
point = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
point_list = [point]
success = self.move(wait=True, point_list=point_list, MAX_LIN_SPD=0.5, MAX_LIN_ACCL=0.75)
return success
# NEED TO TO SWITCH JOINT CONTROL MODE TO JOINT AND USE AN IK SOLVER TO INTERPOLATE JOINT POSITIONS
# FUNCTION TO ABSTRACT CONTORL OF ARM
def move(self, point_list, wait = True, MAX_LIN_SPD=7.0, MAX_LIN_ACCL=1.5): # one point = [x_coord, y_coord, z_coord, x_deg, y_deg, z_deg]
try:
limb = Limb() # point_list = [pointA, pointB, pointC, ...]
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
except:
print("There may have been an error while exiting")
if self.STOP:
traj.stop_trajectory()
return True
wpt_opts = MotionWaypointOptions(max_linear_speed=MAX_LIN_SPD, max_linear_accel=MAX_LIN_ACCL, corner_distance=0.002)
for point in point_list:
q_base = quaternion_from_euler(0, 0, math.pi/2)
#q_rot = quaternion_from_euler(math.radians(point[3]), math.radians(point[4]), math.radians(point[5]))
q_rot = quaternion_from_euler(point[3], point[4], point[5])
q = quaternion_multiply(q_rot, q_base)
newPose = PoseStamped()
newPose.header = Header(stamp=rospy.Time.now(), frame_id='base')
newPose.pose.position.x = point[0] + 0.65
newPose.pose.position.y = point[1] + 0.0
newPose.pose.position.z = point[2] + 0.4
newPose.pose.orientation.x = q[0]
newPose.pose.orientation.y = q[1]
newPose.pose.orientation.z = q[2]
newPose.pose.orientation.w = q[3]
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
waypoint.set_cartesian_pose(newPose, "right_hand", limb.joint_ordered_angles())
traj.append_waypoint(waypoint.to_msg())
if(wait):
print(" \n --- Sending trajectory and waiting for finish --- \n")
result = traj.send_trajectory(wait_for_result=wait)
if result is None:
rospy.logerr('Trajectory FAILED to send')
success = False
elif result.result:
rospy.loginfo('Motion controller successfully finished the trajcetory')
success = True
else:
rospy.logerr('Motion controller failed to complete the trajectory. Error: %s', result.errorId)
success = False
else:
print("\n --- Sending trajector w/out waiting --- \n")
traj.send_trajectory(wait_for_result=wait)
success = True
return success
# MAIN CONTROL LOOP
def fly(self, weather="calm"):
print("Flying")
self.moveToNeutral()
if self.args["box"]:
self.trace_box()
self.moveToNeutral()
if self.args["line"]:
self.follow_line()
self.moveToNeutral()
return
#self.sim_drone()
self.move(wait=True, point_list=[[-0.011,0.046,0.015,-0.53,0.023,-6.5e06],[-0.012, 0.04, 0.014, -0.53, 0.022, -6.2e-06], [0.0, 0.0, 0.0, 0, 0, 0]])
#rate = rospy.Rate(RATE)
#while not rospy.is_shutdown():
# rate.sleep()
return
class Drone:
def __init__(self):
print("Getting robot state... ")
if ACCESS_ROBOT:
self.rs = RobotEnable(CHECK_VERSION)
self.rs.enable()
self.STOP = False
self.pose = PoseStamped()
self.prevX = 0.65
self.prevY = 0.0
self.l = Lights()
self.l_name = 'head_green_light'
self.r_name = 'head_red_light'
ap = argparse.ArgumentParser()
ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough")
ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition (true/false)")
ap.add_argument("-b", "--box", action='store_true', help="Test box edges at progam startup (true/false)")
ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position")
self.args = vars(ap.parse_args())
if self.args["condition"] is None:
self.args["condition"] = "calm"
if not self.args["randomize"]:
self.args["randomize"] = False
if not self.args["box"]:
self.args["box"] = False
outargs = dict(zip(self.args.keys(), self.args.values()))
print('')
print('')
print(outargs)
print('')
print('')
print('')
print('')
self.parser = InputData()
# ENABLE RED LIGHT WHILE WAITING FOR USER INPUT
self.l.set_light_state(self.r_name, on=True)
try:
input(" ----- ROBOT ENABLED, PLEASE PRESS 'ENTER' TO CONTINUE ----- ")
except Exception:
print("")
# ENABLE GREEN LIGHT WHEN PROGRAM IS RUNNING
self.l.set_light_state(self.r_name, on=False)
self.l.set_light_state(self.l_name, on=True)
# ENSURE THE WAYPOINTS LIST IS CLEARED AT TERMINATION OF PROGRAM
def clean_shutdown(self):
print("Stopping arm...")
try:
self.STOP = True
self.move(point_list = None)
self.l.set_light_state(self.l_name, on=False)
except:
print("There may have been an error exiting")
print("Stop successful, exiting...")
return
# INPUT THE SIMULATED TRAJECTORY AND PUSH TO OUTPUT TO move()
def sim_drone(self):
M = self.parser.inputMatrix()
point_list = list()
for x in range(0,350,3): # TIME IN FLIGHT SIM = 0.1*RANGE
point = [ M[x][2][0],-M[x][2][1],-M[x][2][2],-M[x][3][0],M[x][3][1],M[x][3][2] ]
#point = [ M[x][2][0],-M[x][2][1],-M[x][2][2],0,0,0 ]
point_list.append(point)
print(point)
self.move(wait=True, point_list=point_list, MAX_SPD_RATIO=0.8, MAX_JOINT_ACCL=[10.0, 8.0, 10.0, 10.0, 12.0, 12.0, 12.0])
# CONTAINS WAYPOINTS TO TRACE BOX
def trace_box(self):
print("I am tracing a box")
point_list = list()
point_list.append([0.0, 0.25, 0.0, 0.0, 0.0, 0.0])
self.move(point_list=point_list)
point_list = list()
point_list.append([0.0, 0.25, 0.1, 0.0, 0.0, 0.0])
self.move(point_list=point_list)
point_list = list()
point_list.append([0.0, -0.25, 0.1, 0.0, 0.0, 0.0])
self.move(point_list=point_list)
point_list = list()
point_list.append([0.0, -0.25, -0.1, 0.0, 0.0, 0.0])
self.move(point_list=point_list)
point_list = list()
point_list.append([0.0, 0.25, -0.1, 0.0, 0.0, 0.0])
self.move(point_list=point_list)
point_list = list()
point_list.append([0.0, 0.25, 0.0, 0.0, 0.0, 0.0])
self.move(point_list=point_list)
return
# MOVE TO ORIGIN DEFINED IN move()
def moveToNeutral(self):
print("\n --- Returning to neutral position (0.65, 0.0, 0.5, 0.0, 0.0, 0.0) ---")
point = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
point_list = [point]
success = self.move(wait=True, point_list=point_list, MAX_SPD_RATIO=0.2)
return success
# FUNCTION TO ABSTRACT CONTORL OF ARM
def move(self, point_list = None, wait = True, MAX_SPD_RATIO=0.4, MAX_JOINT_ACCL=[7.0, 5.0, 8.0, 8.0, 8.0, 8.0, 8.0]): # one point in point_list = [x_coord, y_coord, z_coord, x_deg, y_deg, z_deg]
try:
limb = Limb() # point_list = [pointA, pointB, pointC, ...]
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.JOINT
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
if self.STOP:
traj.stop_trajectory()
return True
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=MAX_SPD_RATIO, max_joint_accel=MAX_JOINT_ACCL)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
angles = limb.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles=angles)
traj.append_waypoint(waypoint.to_msg())
for point in point_list:
#q_base = quaternion_from_euler(0, math.pi/2, 0)
q_base = quaternion_from_euler(0, 0, 0)
#q_rot = quaternion_from_euler(math.radians(point[3]), math.radians(point[4]), math.radians(point[5])) # USE THIS IF ANGLES ARE IN DEGREES
q_rot = quaternion_from_euler(point[3], point[4], point[5]) # USE THIS IF ANGLES ARE IN RADIANS
q = quaternion_multiply(q_rot, q_base)
# DEFINE ORIGIN
origin = {
'x' : 0.65,
'y' : 0.0,
'z' : 0.4
}
# CREATE CARTESIAN POSE FOR IK REQUEST
newPose = PoseStamped()
newPose.header = Header(stamp=rospy.Time.now(), frame_id='base')
newPose.pose.position.x = point[0] + origin['x']
newPose.pose.position.y = point[1] + origin['y']
newPose.pose.position.z = point[2] + origin['z']
newPose.pose.orientation.x = q[0]
newPose.pose.orientation.y = q[1]
newPose.pose.orientation.z = q[2]
newPose.pose.orientation.w = q[3]
# REQUEST IK SERVICE FROM ROS
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
# SET THE NEW POSE AS THE IK REQUEST
ikreq.pose_stamp.append(newPose)
ikreq.tip_names.append('right_hand')
# ATTEMPT TO CALL THE SERVICE
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except:
print("IK SERVICE CALL FAILED")
return
# HANDLE RETURN
if (resp.result_type[0] > 0):
joint_angles = resp.joints[0].position
# APPEND JOINT ANGLES TO NEW WAYPOINT
waypoint.set_joint_angles(joint_angles=joint_angles)
traj.append_waypoint(waypoint.to_msg())
else:
print("INVALID POSE")
print(resp.result_type[0])
if(wait):
print(" \n --- Sending trajectory and waiting for finish --- \n")
result = traj.send_trajectory(wait_for_result=wait)
if result is None:
rospy.logerr('Trajectory FAILED to send')
success = False
elif result.result:
rospy.loginfo('Motion controller successfully finished the trajcetory')
success = True
else:
rospy.logerr('Motion controller failed to complete the trajectory. Error: %s', result.errorId)
success = False
else:
print("\n --- Sending trajector w/out waiting --- \n")
traj.send_trajectory(wait_for_result=wait)
success = True
return success
except rospy.ROSInterruptException:
print("User interrupt detected. Exiting before trajectory completes")
# MAIN CONTROL LOOP
def fly(self, weather="calm"):
print("Flying")
self.moveToNeutral()
if self.args["reset"]:
self.l.set_light_state(self.l_name, on=False)
return
if self.args["box"]:
self.trace_box()
self.moveToNeutral()
self.sim_drone()
#self.move(wait=True, point_list=[[0,0,0,1.159/2,0,0]])
#rate = rospy.Rate(RATE)
#while not rospy.is_shutdown():
# rate.sleep()
self.l.set_light_state(self.l_name, on=False)
return