def test_ambf_psm(): c = Client() c.connect() time.sleep(2.0) print(c.get_obj_names()) b = c.get_obj_handle('baselink') time.sleep(1.0) # The following are the names of the controllable joints. # 'baselink-yawlink', 0 # 'yawlink-pitchbacklink', 1 # 'pitchendlink-maininsertionlink', 2 # 'maininsertionlink-toolrolllink', 3 # 'toolrolllink-toolpitchlink', 4 # 'toolpitchlink-toolgripper1link', 5a # 'toolpitchlink-toolgripper2link', 5b test_q = [-0.3, 0.2, 0.1, -0.9, 0.0, -1.2, 0.0] T_7_0 = compute_FK(test_q) computed_q = compute_IK(convert_mat_to_frame(T_7_0)) print('SETTING JOINTS: ') print(computed_q) b.set_joint_pos('baselink-yawlink', computed_q[0]) b.set_joint_pos('yawlink-pitchbacklink', computed_q[1]) b.set_joint_pos('pitchendlink-maininsertionlink', computed_q[2]) b.set_joint_pos('maininsertionlink-toolrolllink', computed_q[3]) b.set_joint_pos('toolrolllink-toolpitchlink', computed_q[4]) b.set_joint_pos('toolpitchlink-toolgripper1link', computed_q[5]) b.set_joint_pos('toolpitchlink-toolgripper2link', -computed_q[5]) time.sleep(3.0)
def controller(): # Create a instance of the client _client = Client() _client.connect() # You can print the names of objects found print(_client.get_obj_names()) tool = _client.get_obj_handle('ecm/remotecenterlink') body = _client.get_obj_handle('ecm/baselink') body.set_joint_pos(0, 0) print body.get_children_names() pos = tool.get_pos() y = pos.y while 1: pos = tool.get_pos() x = pos.x print pos tool.set_pos(x + .1, 0.0, 0) time.sleep(0.1)
import time # Import the Client from ambf_client package from ambf_client import Client # Create a instance of the client _client = Client() # Connect the client which in turn creates callable objects from ROS topics # and initiates a shared pool of threads for bi-directional communication _client.connect() print('\n\n----') print("List of Objects") print(_client.get_obj_names()) _handle = _client.get_obj_handle('base') time.sleep(0.2) print('\n\n----') # print(' Base Pos:') # print(_handle.get_pos()) # print(' Base Rotation as Quaternion:') # print(_handle.get_rot()) print('\n\n----') print('Number of Joints in base:')
def main(): # Begin Argument Parser Code parser = ArgumentParser() parser.add_argument('-d', action='store', dest='spacenav_name', help='Specify ros base name of spacenav', default='/spacenav/') parser.add_argument('-o', action='store', dest='obj_name', help='Specify AMBF Obj Name', default='BoxTool') parser.add_argument('-c', action='store', dest='camera_name', help='Specify AMBF Camera Name', default='default_camera') parser.add_argument('-a', action='store', dest='client_name', help='Specify AMBF Client Name', default='client_name') parser.add_argument('-p', action='store', dest='print_obj_names', help='Print Object Names', default=False) parsed_args = parser.parse_args() print('Specified Arguments') print parsed_args _spacenav_one_name = parsed_args.spacenav_name _obj_one_name = parsed_args.obj_name _default_camera_name = parsed_args.camera_name _ambf_client_name = parsed_args.client_name _print_obj_names = parsed_args.print_obj_names client = Client(_ambf_client_name) client.connect() if _print_obj_names: print('Printing Found AMBF Object Names: ') print client.get_obj_names() exit() _pair_one_specified = True _device_pairs = [] default_camera = client.get_obj_handle(_default_camera_name) # The publish frequency _pub_freq = 500 spacenav_one = SpaceNavDevice(_spacenav_one_name, client, _obj_one_name, default_camera) _device_pairs.append(spacenav_one) rate = rospy.Rate(_pub_freq) msg_index = 0 _start_time = rospy.get_time() while not rospy.is_shutdown(): for dev in _device_pairs: dev.process_commands() rate.sleep() msg_index = msg_index + 1 if msg_index % _pub_freq * 3 == 0: # Print every 3 seconds as a flag to show that this code is alive print('SpaceNav AMBF Node Alive...', round(rospy.get_time() - _start_time, 3), 'secs') if msg_index >= _pub_freq * 10: # After ten seconds, reset, no need to keep increasing this msg_index = 0
#ask Mentor about cleaner way of subscribing to several topics def __init__(self,Topic): jointsub = rospy.Subscriber(str(Topic),Topic,self.jointCallback) def jointCallback(self,data): data pass #creates an AMBF client and tries to connects to it try: _client = Client() _client.connect() except: rospy.logwarn("ERROR CONNECTING TO AMBF CLIENT!") else: rospy.loginfo("Created and Connected to AMBF client.") rospy.loginfo(_client.get_obj_names()) RoverBody =_client.get_obj_handle('RoverBody') #Joint Class to (hopefully) make adressing several joints easier. class Joint: obj = None jointidx = 0 def __init__(self, body,joint): #creates an obj for ambf to work with. #Also has ****very basic***** error checking since idk how to do anything more advanced try: Joint.obj = _client.get_obj_handle(body) Joint.jointidx = joint except: rospy.logwarn("An Error Occured while creating joint!")
def test_ambf_ecm(): c = Client('ecm_ik_test') c.connect() time.sleep(2.0) print(c.get_obj_names()) b = c.get_obj_handle('ecm/baselink') target_ik = c.get_obj_handle('ecm/target_ik') target_fk = c.get_obj_handle('ecm/target_fk') time.sleep(1.0) # The following are the names of the controllable joints. # 'baselink-yawlink', 0 # 'yawlink-pitchbacklink', 1 # 'pitchendlink-maininsertionlink', 2 # 'maininsertionlink-toollink', 3 num_joints = 4 joint_lims = np.zeros((num_joints, 2)) joint_lims[0] = [np.deg2rad(-91.96), np.deg2rad(91.96)] joint_lims[1] = [np.deg2rad(-60), np.deg2rad(60)] joint_lims[2] = [0.0, 0.24] joint_lims[3] = [np.deg2rad(-175), np.deg2rad(175)] js_traj = JointSpaceTrajectory(num_joints=num_joints, num_traj_points=50, joint_limits=joint_lims) num_points = js_traj.get_num_traj_points() for i in range(num_points): test_q = js_traj.get_traj_at_point(i) T_4_0 = compute_FK(test_q) if target_ik is not None: P_0_w = Vector(b.get_pos().x, b.get_pos().y, b.get_pos().z) R_0_w = Rotation.RPY(b.get_rpy()[0], b.get_rpy()[1], b.get_rpy()[2]) T_0_w = Frame(R_0_w, P_0_w) T_7_w = T_0_w * convert_mat_to_frame(T_4_0) target_ik.set_pos(T_7_w.p[0], T_7_w.p[1], T_7_w.p[2]) target_ik.set_rpy(T_7_w.M.GetRPY()[0], T_7_w.M.GetRPY()[1], T_7_w.M.GetRPY()[2]) computed_q = compute_IK(convert_mat_to_frame(T_4_0)) # computed_q = enforce_limits(computed_q, joint_lims) if target_fk is not None: P_0_w = Vector(b.get_pos().x, b.get_pos().y, b.get_pos().z) R_0_w = Rotation.RPY(b.get_rpy()[0], b.get_rpy()[1], b.get_rpy()[2]) T_0_w = Frame(R_0_w, P_0_w) T_4_0_fk = compute_FK(computed_q) T_4_w = T_0_w * convert_mat_to_frame(T_4_0_fk) target_fk.set_pos(T_4_w.p[0], T_4_w.p[1], T_4_w.p[2]) target_fk.set_rpy(T_4_w.M.GetRPY()[0], T_4_w.M.GetRPY()[1], T_4_w.M.GetRPY()[2]) # print('SETTING JOINTS: ') # print(computed_q) b.set_joint_pos('baselink-yawlink', computed_q[0]) b.set_joint_pos('yawlink-pitchbacklink', computed_q[1]) b.set_joint_pos('pitchendlink-maininsertionlink', computed_q[2]) b.set_joint_pos('maininsertionlink-toollink', computed_q[3]) test_q = round_vec(test_q) T_4_0 = round_mat(T_4_0, 4, 4, 3) errors = [0] * num_joints for j in range(num_joints): errors[j] = test_q[j] - computed_q[j] print('--------------------------------------') print('**************************************') print('Test Number:', i) print('Joint Positions used to T_EE_B (EndEffector in Base)') print test_q print('Requested Transform for T_EE_B (EndEffector in Base)') print(T_4_0) print('Joint Errors from IK Solver') print["{0:0.2f}".format(k) for k in errors] time.sleep(1.0) time.sleep(3.0)
def main(): # Begin Argument Parser Code parser = ArgumentParser() parser.add_argument('-d', action='store', dest='joystick_name', help='Specify ros base name of joystick', default='/') parser.add_argument('-o', action='store', dest='obj_name', help='Specify AMBF Obj Name', default='Chassis') parser.add_argument('-a', action='store', dest='client_name', help='Specify AMBF Client Name', default='client_name') parser.add_argument('-p', action='store', dest='print_obj_names', help='Print Object Names', default=False) parsed_args = parser.parse_args() print('Specified Arguments') print parsed_args client = Client(parsed_args.client_name) client.connect() if parsed_args.print_obj_names: print('Printing Found AMBF Object Names: ') print client.get_obj_names() exit() control_units = [] num_rovers = 2 num_actuators = 4 # Since we have only one JS for now, joystick = JoyStickDevice(parsed_args.joystick_name) for i in range(num_rovers): rover_prefix = 'rover' + str(i + 1) + '/' rover = client.get_obj_handle(rover_prefix + 'Rover') arm = client.get_obj_handle(rover_prefix + 'arm_link1') sensor = client.get_obj_handle(rover_prefix + 'Proximity0') actuators = [] for j in range(num_actuators): actuator = client.get_obj_handle(rover_prefix + 'Constraint' + str(j)) actuators.append(actuator) # If you have multiple Joysticks, you can pass in different names control_unit = ControlUnit(joystick, rover, arm, sensor, actuators) control_units.append(control_unit) # The publish frequency pub_freq = 60 rate = rospy.Rate(pub_freq) msg_index = 0 while not rospy.is_shutdown(): # Control the active Control Unit control_units[active_rover].process_commands() rate.sleep() msg_index = msg_index + 1 if msg_index % pub_freq * 50 == 0: # Print every 3 seconds as a flag to show that this code is alive # print('Running JoyStick Controller Node...', round(rospy.get_time() - _start_time, 3), 'secs') pass if msg_index >= pub_freq * 10: # After ten seconds, reset, no need to keep increasing this msg_index = 0
import rospy from sensor_msgs.msg import Joy from ambf_client import Client import keyboard #used for keyboard events to read keyboard input. FIND A LIBRARY THAT WORKS import time #Creates a client and connects to it _client = Client() _client.connect() #.set_pos() origin is the center #.set_rpy() uses radiians. Also RPY is roll pitch yaw. print('Client connected\n') print('Listing objects: \n' + str(_client.get_obj_names())) def toRad(deg): #function to convert from degrees to radians since I didn't pay enough attention in triginometry last year return deg * (3.14159 / 180) def main(): #test code -- should rotate armaround in circles armBase_obj = _client.get_obj_handle('ambf/env/armBase') armBase_obj.set_pos(0, 0, 2) armBase_obj.set_rpy(toRad(90), 0, 0) print(armBase_obj.get_num_joints()) print(armBase_obj.get_children_names()) jointnum = input("what joint?")
# \author <http://www.aimlab.wpi.edu> # \author <*****@*****.**> # \author Adnan Munawar # \version 0.1 # */ # //============================================================================== from ambf_client import Client from ambf_msgs.msg import ObjectCmd import time import math obj_name = 'CenterPuzzle' client = Client() client.connect() print client.get_obj_names() obj = client.get_obj_handle(obj_name) if obj: num_jnts = obj.get_num_joints() for i in range(0, 2000): obj.pose_command( 0.5 * math.sin(i/40.0), 0.5 * math.cos(i/40.0), 0, 0, 0, 0) if num_jnts > 0: obj.set_joint_pos(0, 0.5*math.sin(i/20.0)) time.sleep(0.01) else: print obj_name, 'not found'
def temp_controller(): # Create a instance of the client _client = Client() # Connect the client which in turn creates callable objects from ROS topics # and initiates a shared pool of threads for bi-directional communication _client.connect() print('\n\n----') raw_input( "We can see what objects the client has found. Press Enter to continue..." ) # You can print the names of objects found. We should see all the links found print(_client.get_obj_names()) # Lets get a handle to PSM and ECM, as we can see in the printed # object names, 'ecm/baselink' and 'psm/baselink' should exist ecm_handle = _client.get_obj_handle('ecm/toollink') # Let's sleep for a very brief moment to give the internal callbacks # to sync up new data from the running simulator time.sleep(0.2) print('\n\n----') raw_input("Let's Get Some Pose Info. Press Enter to continue...") # Not we can print the pos and rotation of object in the World Frame print('ECM Base Pos:') print(ecm_handle.get_pos()) print('\n\n----') raw_input("Let's get Joints and Children Info. Press Enter to continue...") # We can get the number of children and joints connected to each object as ecm_num_joints = ecm_handle.get_num_joints( ) # Get the number of joints of this object print('Number of Joints in ECM:') print(ecm_num_joints) print(' ') print('Name of PSM\'s children:') print('\n\n----') raw_input("Control ECMs joint positions. Press Enter to continue...") # Now let's control some joints # The 1st joint, which the ECM Yaw ecm_handle.set_joint_pos(0, 0) # The 2nd joint, which is the ECM Pitch print('\n\n----') raw_input( "Mixed Pos and Effort control of PSM\'s joints. Press Enter to continue..." ) print('\n\n----') raw_input( "Set force on MTM's Wrist Yaw link for 5 secs. Press Enter to continue..." ) # Let's directly control the forces and torques on the mtmWristYaw Link # Notice that these are in the world frame. Another important thing to notice # is that unlike position control, forces control requires a continuous update # to meet a watchdog timing condition otherwise the forces are reset Null. This # is purely for safety reasons to prevent unchecked forces in case of malfunctioning # python client code print('\n\n----') raw_input("Let's clean up. Press Enter to continue...") # Lastly to cleanup _client.clean_up()
def test_ambf_mtm(): c = Client('mtm_ik_test') c.connect() time.sleep(2.0) print(c.get_obj_names()) b = c.get_obj_handle('mtm/TopPanel') target_ik = c.get_obj_handle('mtm/target_ik') target_fk = c.get_obj_handle('mtm/target_fk') time.sleep(1.0) # The following are the names of the controllable joints. # 'TopPanel-OutPitchShoulder', 0 # 'OutPitchShoulder-ArmParallel', 1 # 'ArmParallel-BottomArm', 2 # 'BottomArm-WristPlatform', 3 # 'WristPlatform-WristPitch', 4 # 'WristPitch-WristYaw', 5 # 'WristYaw-WristRoll', 6 num_joints = 7 joint_lims = np.zeros((num_joints, 2)) joint_lims[0] = [np.deg2rad(-75), np.deg2rad(44.98)] joint_lims[1] = [np.deg2rad(-44.98), np.deg2rad(44.98)] joint_lims[2] = [np.deg2rad(-44.98), np.deg2rad(44.98)] joint_lims[3] = [np.deg2rad(-59), np.deg2rad(245)] joint_lims[4] = [np.deg2rad(-90), np.deg2rad(180)] joint_lims[5] = [np.deg2rad(-44.98), np.deg2rad(44.98)] joint_lims[6] = [np.deg2rad(-175), np.deg2rad(175)] js_traj = JointSpaceTrajectory(num_joints=7, num_traj_points=50, joint_limits=joint_lims) num_points = js_traj.get_num_traj_points() num_joints = 7 for i in range(num_points): test_q = js_traj.get_traj_at_point(i) T_7_0 = compute_FK(test_q) if target_ik is not None: P_0_w = Vector(b.get_pos().x, b.get_pos().y, b.get_pos().z) R_0_w = Rotation.RPY(b.get_rpy()[0], b.get_rpy()[1], b.get_rpy()[2]) T_0_w = Frame(R_0_w, P_0_w) T_7_w = T_0_w * convert_mat_to_frame(T_7_0) target_ik.set_pos(T_7_w.p[0], T_7_w.p[1], T_7_w.p[2]) target_ik.set_rpy(T_7_w.M.GetRPY()[0], T_7_w.M.GetRPY()[1], T_7_w.M.GetRPY()[2]) computed_q = compute_IK(convert_mat_to_frame(T_7_0)) # computed_q = enforce_limits(computed_q, joint_lims) if target_fk is not None: P_0_w = Vector(b.get_pos().x, b.get_pos().y, b.get_pos().z) R_0_w = Rotation.RPY(b.get_rpy()[0], b.get_rpy()[1], b.get_rpy()[2]) T_0_w = Frame(R_0_w, P_0_w) T_7_0_fk = compute_FK(computed_q) T_7_w = T_0_w * convert_mat_to_frame(T_7_0_fk) target_fk.set_pos(T_7_w.p[0], T_7_w.p[1], T_7_w.p[2]) target_fk.set_rpy(T_7_w.M.GetRPY()[0], T_7_w.M.GetRPY()[1], T_7_w.M.GetRPY()[2]) # print('SETTING JOINTS: ') # print(computed_q) b.set_joint_pos('TopPanel-OutPitchShoulder', computed_q[0]) b.set_joint_pos('OutPitchShoulder-ArmParallel', computed_q[1]) b.set_joint_pos('ArmParallel-BottomArm', computed_q[2]) b.set_joint_pos('BottomArm-WristPlatform', computed_q[3]) b.set_joint_pos('WristPlatform-WristPitch', computed_q[4]) b.set_joint_pos('WristPitch-WristYaw', computed_q[5]) b.set_joint_pos('WristYaw-WristRoll', computed_q[6]) test_q = round_vec(test_q) T_7_0 = round_mat(T_7_0, 4, 4, 3) errors = [0]*num_joints print ('--------------------------------------') print ('**************************************') print ('Test Number:', i) print ('Joint Positions used to T_EE_B (EndEffector in Base)') print test_q print ('Requested Transform for T_EE_B (EndEffector in Base)') print (T_7_0) print ('Joint Errors from IK Solver') print ["{0:0.2f}".format(k) for k in errors] time.sleep(1.0) time.sleep(3.0)