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 __init__(self, mass, height): """ :param name_space: name space of the the body """ super(AMBF, self).__init__(mass, height) _client = Client() _client.connect() time.sleep(2) handle = _client.get_obj_handle('Hip')
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 numpy as np from ambf_client import Client _client = Client() _client.connect() base = _client.get_obj_handle('/ambf/env/neuro_robot/base_link') while 1: base.set_joint_pos(0, 0) base.set_joint_pos(1, 0) base.set_joint_pos(2, 0) base.set_joint_pos(3, 0) base.set_joint_pos(4, 0) base.set_joint_pos(5, 0) base.set_joint_pos(6, 0)
math.sin(math.pi/2 + q4_res), math.cos(math.pi/2 + q4_res)*math.cos(0), -math.cos(math.pi/2 + q4_res)*math.sin(0), a*math.sin(math.pi/2 + q4_res), 0, math.sin(0), math.cos(0), 0, 0, 0, 0, 1]).reshape((4,4)) T_2_5 = np.matmul(np.matmul(np.linalg.inv(shoulder_pose),end_effector_pose),np.linalg.inv(A6)) q2_res = round(math.asin(-T_2_5[2,1]),3) q3_res = round(math.asin(T_2_5[2,2]/math.cos(q2_res)),3) q1_res = round(math.asin(T_2_5[1,1]/math.cos(q2_res)),3) return [q1_res,q2_res,q3_res,q4_res] # Initialize client c = Client() # Connect client and simulation c.connect() time.sleep(2) b = c.get_obj_handle('base') elbow = c.get_obj_handle('elbow') tip = c.get_obj_handle('wrist') # Joints that can be actuated joint_list = [0,1,2,5] base_joint_names = [] for item in joint_list: base_joint_names.append(b.get_joint_names()[item]) print(base_joint_names) # Set all joints to 0 for item in joint_list: b.set_joint_pos(item,0) time.sleep(0.01) time.sleep(3) # Print all joint positions for item in joint_list:
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!") else:
tk = Tkinter.Tk() tk.title("Constraint") tk.geometry("250x250") activate_button_lin = Tkinter.Button(tk, text="Activate", command=activate_cb_lin, height=3, width=50, bg="red") activate_button_lin.pack() c = Client('six_dof_constraint_test') c.connect() box = c.get_obj_handle('Cube') time.sleep(0.5) # Linear constraint_lin_pos = Vector(0, 0, 0) lin_thresh = Vector(1.0, 1.0, 1.0) * 0.001 lin_P_gains = 10.0 lin_D_gains = 0.0 last_lin_error = Vector(0, 0, 0) lin_error = Vector(0, 0, 0) activate_lin_constraint = False lin_proximity_trigger = False # Angular constraint_ang_pos = Vector(0, 0, 0) ang_thresh = Vector(1.0, 1.0, 1.0) * 0.05
class AmbfEnvHERDDPG(gym.GoalEnv): def __init__(self, action_space_limit, joints_to_control, goal_position_range, position_error_threshold, goal_error_margin, joint_limits, workspace_limits, enable_step_throttling): super(AmbfEnvHERDDPG, self).__init__() self.obj_handle = Object self.world_handle = World self.ambf_client = Client() self.ambf_client.connect() time.sleep(5) self.ambf_client.create_objs_from_rostopics() self.seed() self.n_skip_steps = 5 self.enable_step_throttling = enable_step_throttling self.action = [] self.obs = Observation() self.initial_pos = copy.deepcopy(self.obs.cur_observation()[0]) self.cmd_joint_pos = np.zeros(7) self.goal_error_margin = goal_error_margin self.joint_limits = joint_limits self.workspace_limits = workspace_limits self.n_actions = 7 self.action_lims_low = -action_space_limit * np.ones(self.n_actions) self.action_lims_high = action_space_limit * np.ones(self.n_actions) self.action_space = spaces.Box(low=-action_space_limit, high=action_space_limit, shape=(self.n_actions, ), dtype="float32") self.observation_space = spaces.Dict( dict( desired_goal=spaces.Box( -np.inf, np.inf, shape=self.initial_pos['achieved_goal'].shape, dtype='float32'), achieved_goal=spaces.Box( -np.inf, np.inf, shape=self.initial_pos['achieved_goal'].shape, dtype='float32'), observation=spaces.Box( -np.inf, np.inf, shape=self.initial_pos['observation'].shape, dtype='float32'), )) self.joints_to_control = joints_to_control self.goal_position_range = goal_position_range self.goal = np.array([0.0, 0.0, -0.1, 0.0, 0.0, 0.0]) self.prev_sim_step = 0 self.pos_error_threshold = position_error_threshold self.count_for_print = 0 def skip_sim_steps(self, num): # Function to define the number of steps that can be skipped if Step Throttling is enabled self.n_skip_steps = num self.world_handle.set_num_step_skips(num) def set_throttling_enable(self, check): # Function to set the Step Throttling Boolean self.enable_step_throttling = check self.world_handle.enable_throttling(check) def make(self, a_name): # Function to create object handle for the robot and world in AMBF self.obj_handle = self.ambf_client.get_obj_handle(a_name) # self.base_handle = self.ambf_client.get_obj_handle('SampleObj') self.world_handle = self.ambf_client.get_world_handle() self.world_handle.enable_throttling(self.enable_step_throttling) self.world_handle.set_num_step_skips(self.n_skip_steps) time.sleep(2) if self.obj_handle is None or self.world_handle is None: raise Exception def seed(self, seed=None): # Random seed self.np_random, seed = seeding.np_random(seed) return [seed] def reset(self): # Function to reset the model # Sets the initial position of PSM self.set_initial_pos_func() # Type 1 Reset : Uses the previous reached state as the initial state for next iteration # action = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # observation, _, _, _ = self.step(action) # Type 2 Reset : Sets the robot to a predefined initial state for each iteration initial_joint_pos, initial_state_vel = self.get_joint_pos_vel_func() end_effector_frame = compute_FK(initial_joint_pos) # Updates the observation to the initialized position observation, _, _, _ = self._update_observation( end_effector_frame, initial_joint_pos, initial_state_vel) # Samples a goal self.goal = self._sample_goal(observation) return observation def set_initial_pos_func(self): # Function to set the initial joint positions of the robot for joint_idx, jt_name in enumerate(self.joints_to_control): # Prismatic joint is set to different value to ensure at least some part of robot tip goes past the cannula if joint_idx == 2: self.obj_handle.set_joint_pos( jt_name, self.joint_limits['lower_limit'][2]) else: self.obj_handle.set_joint_pos(jt_name, 0) time.sleep(0.5) def get_joint_pos_vel_func(self): # Function to compute Joint Position and Velocity of the robot joint_position = np.zeros(7) joint_velocity = np.zeros(7) for joint_idx, jt_name in enumerate(self.joints_to_control): joint_position[joint_idx] = self.obj_handle.get_joint_pos(jt_name) joint_velocity[joint_idx] = self.obj_handle.get_joint_vel(jt_name) return joint_position, joint_velocity def step(self, action): assert len(action) == 7 # Counter for printing position, action and reward self.count_for_print += 1 # Initialization of variables new_state_joint_pos = np.zeros(7) # Clipping the action value between the desired range action = np.clip(action, self.action_lims_low, self.action_lims_high) current_joint_pos, state_vel = self.get_joint_pos_vel_func() # Take the action from current state to reach the new state for joint_idx, jt_name in enumerate(self.joints_to_control): new_state_joint_pos[joint_idx] = np.add( current_joint_pos[joint_idx], action[joint_idx]) # Ensure the new state is within valid joint positions, if invalid then stay at the joint limit position desired_joint_pos = self.limit_joint_pos(new_state_joint_pos) # Ensures that PSM joints reach the desired joint positions self.set_commanded_joint_pos(desired_joint_pos) current_end_effector_frame = compute_FK(desired_joint_pos) # Update state, reward, done flag and world values in the code updated_state, rewards, done, info = self._update_observation( current_end_effector_frame, desired_joint_pos, state_vel) self.world_handle.update() # Print function for viewing the output intermittently if self.count_for_print % 10000 == 0: print("count ", self.count_for_print, "Action is ", action, " new pos after action ", updated_state) print("Reward is ", rewards) return updated_state, rewards, done, info def set_commanded_joint_pos(self, commanded_joint_pos): # Function to ensure the robot tip reaches the desired goal position before moving on to next iteration # Counter to avoid getting stuck in while loop because of very small errors in positions count_for_joint_pos = 0 while True: reached_joint_pos = np.zeros(7) for joint_idx, jt_name in enumerate(self.joints_to_control): self.obj_handle.set_joint_pos(jt_name, commanded_joint_pos[joint_idx]) reached_joint_pos[joint_idx] = self.obj_handle.get_joint_pos( jt_name) error_in_pos = np.around(np.subtract(commanded_joint_pos, reached_joint_pos), decimals=3) # Since Prismatic joint limits are smaller compared to the limits of other joints error_in_pos_joint2 = np.around(np.subtract( commanded_joint_pos[2], reached_joint_pos[2]), decimals=4) count_for_joint_pos += 1 if (np.all(np.abs(error_in_pos) <= self.pos_error_threshold) and np.abs(error_in_pos_joint2) <= 0.5*self.pos_error_threshold) \ or count_for_joint_pos > 75: break def limit_cartesian_pos(self, cart_pos): # Limits the robot tip X, Y, Z positions limit_cartesian_pos_values = np.zeros(3) cartesian_pos_lower_limit = self.workspace_limits['lower_limit'] cartesian_pos_upper_limit = self.workspace_limits['upper_limit'] for axis in range(3): limit_cartesian_pos_values[axis] = np.clip( cart_pos[axis], cartesian_pos_lower_limit[axis], cartesian_pos_upper_limit[axis]) return limit_cartesian_pos_values def limit_joint_pos(self, joint_pos): # Limits the joint position values of the robot # dvrk_limits_low = np.array([-1.605, -0.93556, -0.002444, -3.0456, -3.0414, -3.0481, -3.0498]) # dvrk_limits_high = np.array([1.5994, 0.94249, 0.24001, 3.0485, 3.0528, 3.0376, 3.0399]) # Note: Joint 5 and 6, joint pos = 0, 0 is closed jaw and 0.5, 0.5 is open limit_joint_values = np.zeros(7) joint_lower_limit = self.joint_limits['lower_limit'] joint_upper_limit = self.joint_limits['upper_limit'] for joint_idx in range(len(joint_pos)): limit_joint_values[joint_idx] = np.clip( joint_pos[joint_idx], joint_lower_limit[joint_idx], joint_upper_limit[joint_idx]) return limit_joint_values def render(self, mode): # Not being utilized since AMBF runs in parallel for visualization print(' I am {} Ironman'.format(mode)) def _update_observation(self, end_effector_frame, joint_pos, state_vel): # Function to update all the values # Function implementing Step Throttling algorithm if self.enable_step_throttling: step_jump = 0 while step_jump < self.n_skip_steps: step_jump = self.obj_handle.get_sim_step() - self.prev_sim_step time.sleep(0.00001) self.prev_sim_step = self.obj_handle.get_sim_step() if step_jump > self.n_skip_steps: print('WARN: Jumped {} steps, Default skip limit {} Steps'. format(step_jump, self.n_skip_steps)) else: cur_sim_step = self.obj_handle.get_sim_step() step_jump = cur_sim_step - self.prev_sim_step self.prev_sim_step = cur_sim_step # Find the tip position and rotation by computing forward kinematics (not being used currently) cartesian_pos = self.limit_cartesian_pos( end_effector_frame[0:3, 3]).reshape((3, 1)) achieved_rot = np.array( euler_from_matrix(end_effector_frame[0:3, 0:3], axes='szyx')).reshape((3, 1)) # Combine tip position and rotation to a single numpy array as achieved goal achieved_goal = np.asarray( np.concatenate((cartesian_pos.copy(), achieved_rot.copy()), axis=0)).reshape(-1) obs = np.asarray( np.concatenate((cartesian_pos.copy(), achieved_rot.copy(), joint_pos.reshape((7, 1))), axis=0)).reshape(-1) # Combine the current state positions and velocity into a single array as observation observation = np.concatenate((obs, state_vel), axis=None) # Update the observation dictionary self.obs.state.update(observation=observation.copy(), achieved_goal=achieved_goal.copy(), desired_goal=self.goal.copy()) # Update info self.obs.info = self._update_info() # Compute the reward self.obs.reward = self.compute_reward(self.obs.state['achieved_goal'], self.goal, self.obs.info) self.obs.is_done = self._check_if_done() # Return the values to step function return self.obs.state, self.obs.reward, self.obs.is_done, self.obs.info def compute_reward(self, achieved_goal, goal, info): # Function to compute reward received by the agent # Find the distance between goal and achieved goal cur_dist = LA.norm(np.subtract(goal[0:3], achieved_goal[0:3])) # Continuous reward # reward = round(1 - float(abs(cur_dist)/0.3)*0.5, 5) # Sparse reward if abs(cur_dist) < 0.01: reward = 1 else: reward = -1 self.obs.dist = cur_dist return reward def _sample_goal(self, observation): # Function to samples new goal positions and ensures its within the workspace of PSM rand_val_pos = np.around(np.add( observation['achieved_goal'][0:3], self.np_random.uniform(-self.goal_position_range, self.goal_position_range, size=3)), decimals=4) rand_val_pos[0] = np.around(np.clip( rand_val_pos[0], self.workspace_limits['lower_limit'][0], self.workspace_limits['upper_limit'][0]), decimals=4) rand_val_pos[1] = np.around(np.clip( rand_val_pos[1], self.workspace_limits['lower_limit'][1], self.workspace_limits['upper_limit'][1]), decimals=4) rand_val_pos[2] = np.around(np.clip( rand_val_pos[2], self.workspace_limits['lower_limit'][2], self.workspace_limits['upper_limit'][2]), decimals=4) # Uncomment below lines if individual limits need to be set for generating desired goal state ''' rand_val_pos[0] = np.around(np.clip(rand_val_pos[0], -0.1388, 0.1319), decimals=4) rand_val_pos[1] = np.around(np.clip(rand_val_pos[1], -0.1319, 0.1388), decimals=4) rand_val_pos[2] = np.around(np.clip(rand_val_pos[2], -0.1935, -0.0477), decimals=4) rand_val_angle[0] = np.clip(rand_val_angle[0], -0.15, 0.15) rand_val_angle[1] = np.clip(rand_val_angle[1], -0.15, 0.15) rand_val_angle[2] = np.clip(rand_val_angle[2], -0.15, 0.15) ''' # Provide the range for generating the desired orientation at the terminal state rand_val_angle = self.np_random.uniform(-1.5, 1.5, size=3) goal = np.concatenate((rand_val_pos, rand_val_angle), axis=None) return goal.copy() def _check_if_done(self): # Function to check if the episode was successful if abs(self.obs.dist) < self.goal_error_margin: return True else: return False def _update_info(self): # Can be used to Provide information for debugging purpose info = {'is_success': self._is_success()} return info def _is_success(self): # Function to check if the robot reached the desired goal within a predefined error margin if abs(self.obs.dist) < self.goal_error_margin: return True else: return False
from ambf_client import Client import time # 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 bidrectional communication _client.connect() # You can print the names of objects found print(_client.get_obj_names()) # Lets say from the list of printed names, we want to get the # handle to an object named "Torus" torus_obj = _client.get_obj_handle('Torus') # Now you can use the torus_obj to set and get its position, rotation, # Pose etc. If the object has joints, you can also control them # in either position control mode or open loop effort mode. You can even mix and # match the joints commands torus_obj.set_pos(0, 0, 0) # Set the XYZ Pos in obj's World frame torus_obj.set_rpy(1.5, 0.7, .0) # Set the Fixed RPY in World frame time.sleep( 5) # Sleep for a while to see the effect of the command before moving on # Other methods to control the obj position include # torus_obj.set_pose(pose_cmd) # Where pose_cmd is of type Geometry_msgs/Pose # torus_obj.set_rot(quaterion) # Where quaternion is a list in the order of [qx, qy, qz, qw] # Finally all the position control params can be controlled in a single method call # torus_obj.pose_command(px, py, pz, roll, pitch, yaw, *jnt_cmds)
class AmbfPSMEnv(gym.GoalEnv): def __init__(self, n_actions, n_states, n_goals): self.obj_handle = Object self.world_handle = World self.ambf_client = Client() self.ambf_client.create_objs_from_rostopics() self.n_skip_steps = 5 self.enable_step_throttling = True self.action = [] self.obs = Observation(n_states=n_states) self.action_lims_low = np.array( [-0.1, -0.1, -0.1, -0.1, -0.1, -0.1, -0.1]) self.action_lims_high = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) # self.action_space = spaces.Box(self.action_lims_low, self.action_lims_high) self.observation_space = spaces.Box(-np.inf, np.inf, shape=(n_states, )) self.action_space = spaces.Box(-1., 1., shape=(n_actions, ), dtype='float32') # For applying HER # self.observation_space = spaces.Dict(dict( # desired_goal=spaces.Box(-np.inf, np.inf, shape=(n_goals,), dtype='float32'), # achieved_goal=spaces.Box(-np.inf, np.inf, shape=(n_goals,), dtype='float32'), # observation=spaces.Box(-np.inf, np.inf, shape=(n_states,), dtype='float32'), # )) # self.base_handle = self.ambf_client.get_obj_handle('PegBase') self.prev_sim_step = 0 pass def skip_sim_steps(self, num): self.n_skip_steps = num self.world_handle.set_num_step_skips(num) def set_throttling_enable(self, check): self.enable_step_throttling = check self.world_handle.enable_throttling(check) def make(self, a_name): self.obj_handle = self.ambf_client.get_obj_handle(a_name) self.world_handle = self.ambf_client.get_world_handle() self.world_handle.enable_throttling(self.enable_step_throttling) self.world_handle.set_num_step_skips(self.n_skip_steps) if self.obj_handle is None or self.world_handle is None: raise Exception def reset(self): action = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] return self.step(action)[0] def step(self, action): assert len(action) == 7 action = np.clip(action, self.action_lims_low, self.action_lims_high) self.action = action self.obj_handle.pose_command(action[0], action[1], action[2], action[3], action[4], action[5], action[6]) self.world_handle.update() self._update_observation(action) return self.obs.cur_observation() def render(self, mode): print(' I am a {} POTATO'.format(mode)) def _update_observation(self, action): if self.enable_step_throttling: step_jump = 0 while step_jump < self.n_skip_steps: step_jump = self.obj_handle.get_sim_step() - self.prev_sim_step time.sleep(0.00001) self.prev_sim_step = self.obj_handle.get_sim_step() if step_jump > self.n_skip_steps: print('WARN: Jumped {} steps, Default skip limit {} Steps'. format(step_jump, self.n_skip_steps)) else: cur_sim_step = self.obj_handle.get_sim_step() step_jump = cur_sim_step - self.prev_sim_step self.prev_sim_step = cur_sim_step state = self.obj_handle.get_pose() + [step_jump] self.obs.state = state self.obs.reward = self._calculate_reward(state, action) self.obs.is_done = self._check_if_done() self.obs.info = self._update_info() def _calculate_reward(self, state, action): prev_dist = self.obs.dist cur_dist = LA.norm(np.subtract(state[6:9], state[0:3])) action_penalty = np.sum(np.square(action)) reward = (prev_dist - cur_dist) - 4 * action_penalty self.obs.dist = cur_dist return reward def _check_if_done(self): return False def _update_info(self): return {}
class AMBF_controller: """Main class for ambf controller """ chain = None solver = None ambf_client = None robot_handle = None def __init__(self, yaml_file): """Initializer for the AMBF class Arguments: yaml_file {string} -- Path of the YAML file to load for kinematic chain """ data = None with open(yaml_file, 'rb') as f: data = yaml.load(f) if 'solver' in data: solver_to_use = data['solver'] else: # Exiting if no solver is defined sys.exit("No Solver is defined. Exiting..") # =============================================================================== Load data, solver # Generating Chain from yaml self.chain = Chain(data) # Get Solver Collection solverCollection = SolverCollection() log.debug("Kinematics Solvers detected :") log.debug(solverCollection.getAllSolvers()) # Get an instance of the solver to be used self.solver = solverCollection.getSolver( solver_to_use, self.chain, strict=True) log.debug("Loaded Solver >> " + self.solver.name) rospy.Subscriber("/ambf/validate", Pose, self.set_pose_callback) # =============================================================================== Connect to AMBF # Create a instance of the client self.ambf_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 self.ambf_client.connect() # Get handle to set joint positions to self.robot_handle = self.ambf_client.get_obj_handle( self.chain.getBaseBody().name) self.robot_tip_handle = self.ambf_client.get_obj_handle( self.chain.getTipBody().name) # =============================================================================== Start ROS comm rospy.Subscriber("/ambf/setPose", Pose, self.set_pose_callback) rospy.Subscriber("/ambf/setJointParams", Float64MultiArray, self.set_joint_params_callback) rate = rospy.Rate(10) while not rospy.is_shutdown(): # print(self.robot_tip_handle.get_pos()) rate.sleep() # rospy.spin() def set_pose_callback(self, msg): # Build a Pose pose = SolverPose(msg.position.x, msg.position.y, msg.position.z, msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w) self.solver.set_current_jointstates(self.robot_handle.get_all_joint_pos()) joint_states = self.solver.solve_for_ik_pos(pose) log.info("Joint Params calculated:") log.info(joint_states) log.info(pose) i = 0 for joint_value in joint_states: self.robot_handle.set_joint_pos(i, joint_value) i += 1 def set_joint_params_callback(self, msg): pose = self.solver.solve_for_fk_pos(msg.data) log.info("Pose calculated:") log.info(pose) for i in range(len(msg.data)): self.robot_handle.set_joint_pos(i, msg.data[i])
def set_point_control(req): _client = Client() _client.connect() base = _client.get_obj_handle('/ambf/env/neuro_robot/base_link') base.set_joint_pos(0, 0) base.set_joint_pos(1, 0) base.set_joint_pos(2, 0) base.set_joint_pos(3, 0) base.set_joint_pos(4, 0) base.set_joint_pos(5, 0) base.set_joint_pos(6, 0) joint_pos = np.array(req.pose, dtype='float64') cfm = MRI_collide_detection(joint_pos) cfh = Head_collide_detection(joint_pos) if cfm[0] == 1 or cfh[0] == 1: sys.exit(1) wm = np.array([cfm[0]], dtype='float64') wh = np.array([cfh[0]], dtype='float64') t = np.array([0], dtype='float64') # current joint pose c_pos = np.array([0, 0, 0, 0, 0, 0, 0]) # ending joint pose e_pos = joint_pos / 1000 # step joint pose s_pos = joint_pos / 100000 # we will rotate the robot while slowly pull out the needle first if not (joint_pos[2] == 0 and joint_pos[3] == 0 and joint_pos[4] == 0 and joint_pos[5] == 0): dinsert = 40 / 100000 while np.mean(abs(e_pos[2:4] - c_pos[2:4])) > 0.0002: # first check the next point is safe n_pos = c_pos n_pos[2] = n_pos[2] + s_pos[2] n_pos[3] = n_pos[3] + s_pos[3] n_pos[4] = n_pos[4] + dinsert n_pos[5] = n_pos[5] + s_pos[5] cfmr = MRI_collide_detection(n_pos) cfhr = Head_collide_detection(n_pos) if not (cfmr[0] == 1 or cfhr[0] == 1): base.set_joint_pos(2, c_pos[2] + s_pos[2]) base.set_joint_pos(3, c_pos[3] + s_pos[3]) base.set_joint_pos(4, c_pos[4] + dinsert) base.set_joint_pos(5, c_pos[5] + s_pos[5]) c_pos[2] = base.get_joint_pos(2) c_pos[3] = base.get_joint_pos(3) c_pos[4] = base.get_joint_pos(4) c_pos[5] = base.get_joint_pos(5) elif cfmr[0] == 1 and cfhr[0] != 1: base.set_joint_pos(2, c_pos[2] + s_pos[2]) base.set_joint_pos(3, c_pos[3] + s_pos[3]) base.set_joint_pos(4, c_pos[4] - dinsert) base.set_joint_pos(5, c_pos[5] + s_pos[5]) c_pos[2] = base.get_joint_pos(2) c_pos[3] = base.get_joint_pos(3) c_pos[4] = base.get_joint_pos(4) c_pos[5] = base.get_joint_pos(5) np.append(wm, cfmr[0]) np.append(wh, cfhr[0]) np.append(t, t[-1] + 0.05) time.sleep(0.048) dinsert = e_pos[4] - c_pos[4] while np.mean(abs(e_pos - c_pos)) > 0.0002: # first check the next point is safe n_pos = c_pos n_pos[0] = n_pos[0] + s_pos[0] n_pos[1] = n_pos[1] + s_pos[1] n_pos[4] = n_pos[4] + dinsert n_pos[6] = n_pos[6] + s_pos[6] cfmr = MRI_collide_detection(n_pos) cfhr = Head_collide_detection(n_pos) if not (cfmr[0] == 1 or cfhr[0] == 1): base.set_joint_pos(0, c_pos[0] + s_pos[0]) base.set_joint_pos(1, c_pos[1] + s_pos[1]) base.set_joint_pos(4, c_pos[4] + dinsert) base.set_joint_pos(6, c_pos[6] + s_pos[6]) c_pos[0] = base.get_joint_pos(0) c_pos[1] = base.get_joint_pos(1) c_pos[4] = base.get_joint_pos(4) c_pos[6] = base.get_joint_pos(6) np.append(wm, cfmr[0]) np.append(wh, cfhr[0]) np.append(t, t[-1] + 0.05) time.sleep(0.048) else: print("Trajectory error") sys.exit(1) return {'wm': wm, 'wh': wh, 't': t}
# \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()
import time import numpy as np import copy from PSM import ForwardKinematics, ik, InverseKinematics import draw_helix if __name__ == '__main__': # 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() psm_handle = _client.get_obj_handle('psm/baselink') # yawlink(0), pitchbacklink(1), maininsertionlink(4), toolrolllink(5), toolpitchlink(6), toolgripper1link(7) # print(' ') # print('PSM Base Rotation as Quaternion:') # print(psm_handle.get_rot()) # print(psm_handle.get_joint_pos(0)) psm_joint_names = psm_handle.get_joint_names( ) # Get a list of children names belonging to this obj print(psm_joint_names) # psm_handle.set_joint_pos(1, 0.5) time.sleep(0.5) # q = np.array([psm_handle.get_joint_pos(0), psm_handle.get_joint_pos(1), psm_handle.get_joint_pos(4), # psm_handle.get_joint_pos(5), psm_handle.get_joint_pos(6), psm_handle.get_joint_pos(7)]) q = np.array([0, 0.5 * 3.14, 0.5, 0, 0, 0]) F = ForwardKinematics(q)
elif parsed_args.run_psm_one in ['False', 'false', '0']: parsed_args.run_psm_one = False if parsed_args.run_psm_two in ['True', 'true', '1']: parsed_args.run_psm_two = True elif parsed_args.run_psm_two in ['False', 'false', '0']: parsed_args.run_psm_two = False if parsed_args.run_psm_three in ['True', 'true', '1']: parsed_args.run_psm_three = True elif parsed_args.run_psm_three in ['False', 'false', '0']: parsed_args.run_psm_three = False c = Client() c.connect() cam_frame = c.get_obj_handle('CameraFrame') time.sleep(0.5) P_c_w = cam_frame.get_pos() R_c_w = cam_frame.get_rpy() T_c_w = Frame(Rotation.RPY(R_c_w[0], R_c_w[1], R_c_w[2]), Vector(P_c_w.x, P_c_w.y, P_c_w.z)) print(T_c_w) controllers = [] psm_arms = [] if parsed_args.run_psm_one is True: # Initial Target Offset for PSM1 # init_xyz = [0.1, -0.85, -0.15] arm_name = 'psm1' print('LOADING CONTROLLER FOR ', arm_name)
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
from ambf_client import Client import time c = Client() c.connect() time.sleep(1.0) # print c.get_obj_names() time.sleep(1.0) b = c.get_obj_handle('baselink') print b.get_name()
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)
#! /usr/bin/env python import sys import rospy # 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() _handle = _client.get_obj_handle('base') _tip = _client.get_obj_handle('link7') print('\n\n----') rate = rospy.Rate(10) while not rospy.is_shutdown(): position = _tip.get_pos() string = str(rospy.get_time()) + "\t x= " + str( position.x) + ",\t y= " + str(position.y) + ",\t z= " + str(position.z) sys.stdout.write('\r' + string) sys.stdout.flush() # important rate.sleep() print('shutdown complete')
# 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 psm_handle_base = _client.get_obj_handle('psm/baselink') psm_handle_yaw = _client.get_obj_handle('psm/yawlink') psm_handle_mi = _client.get_obj_handle('psm/maininsertionlink') psm_handle_pfl = _client.get_obj_handle('psm/pitchfrontlink') psm_handle_pel = _client.get_obj_handle('psm/pitchendlink') # 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('PSM Base Pos:') print(psm_handle_base.get_pos())
# 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()) #Let's label the baselinks for the left and right arm of Raven2 l_handle = _client.get_obj_handle('raven_2/base_link_L') r_handle = _client.get_obj_handle('raven_2/base_link_R') ''' #Next, let's label the wrists for the left and right arm of the Raven2 l_wrist_handle = _client.get_obj_handle('/ambf/env/raven_2/wrist_L') r_wrist_handle = _client.get_obj_handle('/ambf/env/raven_2/wrist_R') #Each link for each arm... link_0 = _client.get_obj_handle('/ambf/env/raven_2/0_link') l_link_1 = _client.get_obj_handle('/ambf/env/raven_2/link1_L') l_link_2 = _client.get_obj_handle('/ambf/env/raven_2/link2_L') l_link_3 = _client.get_obj_handle('/ambf/env/raven_2/link3_L') r_link_1 = _client.get_obj_handle('/ambf/env/raven_2/link1_R') r_link_2 = _client.get_obj_handle('/ambf/env/raven_2/link2_R')
from ambf_client import Client import rospy from sensor_msgs.msg import Joy from PyKDL import Vector, Wrench, Rotation import math import time _client = Client() _client.connect() Body = _client.get_obj_handle("RoverBody") rate = rospy.Rate(1) Body.set_pos(0, 0, 0) Body.set_rpy(math.pi / 2, 0, 0) while not rospy.is_shutdown(): #Body.set_pos(0,0,0) Body.set_rpy(math.pi / 2, 0, 0) Body.set_pos(input(" in x: "), input(" in y: "), 0) rospy.sleep(0.5) print(Body.get_pos()) rate.sleep()
import time import matplotlib.pyplot as plt depth = -1.4 scale = 1.0 ts = 1.0 / 1000.0 episode = 20.0 dt = 0.01 trajectory_type = 1 y_min = -1. y_max = 1. c = Client() c.connect() box = c.get_obj_handle('BoxTool') # diff = c.get_obj_handle('diff') box.set_rpy(0, -1.57079, 0) n_steps = int(episode / dt) t = 0 t_series = [] error_series = [] # sinosoidal trajectory if trajectory_type == 0: box.set_pos(0, 0, depth) time.sleep(2.0) y_start = box.get_pos().y
from sensor_msgs.msg import PointCloud import numpy as np from ambf_client import Client import rospy from geometry_msgs.msg import Point32 client = Client() client.connect() sensor = client.get_obj_handle("/ambf/env/Prox") rospy.sleep(3) pub = rospy.Publisher('cloud', PointCloud, queue_size=10) rate = rospy.Rate(1000) theta = np.linspace(0, 2 * np.pi, 10) while 1: ranges = sensor.get_all_measurements() range = sensor.get_range(0) print(range) parent = sensor.get_parent() print(parent) pose = sensor.get_pose() print(pose) msg = PointCloud() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "/stair" #print(range)
# 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----') 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/baselink') psm_handle = _client.get_obj_handle('psm/baselink') # Similarly we can get a handle to any link lower in the hierarchy rather # than the root link. Let's get a handle to MTMs wrist platform link mtm_wrist_handle = _client.get_obj_handle('mtm/WristYaw') # 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----') 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())
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
def set_point_control(req): _client = Client() _client.connect() base = _client.get_obj_handle('/ambf/env/neuro_robot/base_link') base.set_joint_pos(0, 0) base.set_joint_pos(1, 0) base.set_joint_pos(2, 0) base.set_joint_pos(3, 0) base.set_joint_pos(4, 0) base.set_joint_pos(5, 0) base.set_joint_pos(6, 0) time.sleep(1) print("Recieved joint positions") print(req.pose) joint_pos = np.array(req.pose, dtype=np.float32) cfm = MRI_collide_detection(joint_pos) cfh = Head_collide_detection(joint_pos) wm = [cfm[0]] wh = [cfh[0]] t = [0] if cfm[0] == 1 or cfh[0] == 1: wm = cfm[1] wh = cfh[1] print("Destination is not reachable without collision") return {'wm': wm, 'wh': wh, 't': t} else: print("Destination is reachable without collision") # current joint pose c_pos = np.array([ .00000000, .00000000, .00000000, .0000000, .00000000, .00000000, .0000000 ]) # ending joint pose e_pos = joint_pos e_pos[0] = e_pos[0] / 1000 e_pos[1] = e_pos[1] / 1000 e_pos[2] = e_pos[2] e_pos[3] = e_pos[3] e_pos[4] = -e_pos[4] / 1000 e_pos[6] = -e_pos[6] / 1000 # step joint pose s_pos = e_pos / 100 print(s_pos) print("starting") # we will rotate the robot while slowly pull out the needle first if joint_pos[2] != 0 or joint_pos[3] != 0: dinsert = -40 / 100000 i = 0 while i <= 100: # first check the next point is safe n_pos = c_pos n_pos[2] = n_pos[2] + s_pos[2] n_pos[3] = n_pos[3] + s_pos[3] n_pos[4] = n_pos[4] + dinsert n_pos[5] = n_pos[5] + s_pos[5] cfmr = MRI_collide_detection(n_pos) cfhr = Head_collide_detection(n_pos) if cfmr[0] != 1 and cfhr[0] != 1: base.set_joint_pos(3, n_pos[2]) base.set_joint_pos(2, n_pos[3]) base.set_joint_pos(4, n_pos[4]) base.set_joint_pos(5, n_pos[5]) c_pos[2] = base.get_joint_pos(2) c_pos[3] = base.get_joint_pos(3) c_pos[4] = base.get_joint_pos(4) c_pos[5] = base.get_joint_pos(5) elif cfmr[0] == 1 and cfhr[0] != 1: base.set_joint_pos(2, n_pos[2]) base.set_joint_pos(3, n_pos[3]) base.set_joint_pos(4, c_pos[4] - dinsert) base.set_joint_pos(5, n_pos[5]) c_pos[2] = base.get_joint_pos(2) c_pos[3] = base.get_joint_pos(3) c_pos[4] = base.get_joint_pos(4) c_pos[5] = base.get_joint_pos(5) else: print("Trajectory error") sys.exit(1) wm.append(cfmr[0]) wh.append(cfhr[0]) t.append(t[-1] + 0.1) i = i + 1 time.sleep(0.1) dinsert = (e_pos[4] - c_pos[4]) / 100 i = 0 while i <= 100: # first check the next point is safe n_pos = c_pos n_pos[0] = n_pos[0] + s_pos[0] n_pos[1] = n_pos[1] + s_pos[1] n_pos[4] = n_pos[4] + dinsert n_pos[6] = n_pos[6] + s_pos[6] cfmr = MRI_collide_detection(n_pos) cfhr = Head_collide_detection(n_pos) if cfmr[0] != 1 or cfhr[0] != 1: base.set_joint_pos(0, n_pos[0]) base.set_joint_pos(1, n_pos[1]) base.set_joint_pos(4, n_pos[4]) base.set_joint_pos(6, n_pos[6]) c_pos[0] = base.get_joint_pos(0) c_pos[1] = base.get_joint_pos(1) c_pos[4] = base.get_joint_pos(4) c_pos[6] = base.get_joint_pos(6) wm.append(cfmr[0]) wh.append(cfhr[0]) t.append(t[-1] + 0.1) i = i + 1 time.sleep(0.1) else: print("Trajectory error") sys.exit(1) wm = np.array(wm, dtype=np.float32) wh = np.array(wh, dtype=np.float32) t = np.array(t, dtype=np.float32) return {'wm': wm, 'wh': wh, 't': t}
# 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:') print(_handle.get_num_joints())
def psm1_btn_cb(): attach_needle(needle, link1) def psm2_btn_cb(): attach_needle(needle, link2) def psm3_btn_cb(): attach_needle(needle, link3) c = Client('attach_needle') c.connect() # psm_name = needle = c.get_obj_handle('Needle') link1 = c.get_obj_handle('psm1' + '/toolyawlink') link2 = c.get_obj_handle('psm2' + '/toolyawlink') link3 = c.get_obj_handle('psm3' + '/toolyawlink') tk = Tkinter.Tk() tk.title("Attache Needle") tk.geometry("250x250") link1_button = Tkinter.Button(tk, text="PSM 1", command=psm1_btn_cb, height=3, width=50, bg="red") link2_button = Tkinter.Button(tk, text="PSM 2",