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",