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