def __init__(self):
self.t = time.time()
self.__AUTOCAMERA_MODE__ = self.MODE.simulation
self.autocamera = Autocamera() # Instantiate the Autocamera Class
self.jnt_msg = JointState()
self.joint_angles = {'ecm':None, 'psm1':None, 'psm2':None}
self.cam_info = {'left':CameraInfo(), 'right':CameraInfo()}
self.last_ecm_jnt_pos = None
self.first_run = True
# For forward and inverse kinematics
self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
self.mtml_robot = URDF.from_parameter_server('/dvrk_mtml/robot_description')
self.mtml_kin = KDLKinematics(self.mtml_robot, self.mtml_robot.links[0].name, self.mtml_robot.links[-1].name)
self.mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description')
self.mtmr_kin = KDLKinematics(self.mtmr_robot, self.mtmr_robot.links[0].name, self.mtmr_robot.links[-1].name)
# For camera clutch control
self.camera_clutch_pressed = False
self.ecm_manual_control_lock_mtml_msg = None
self.ecm_manual_control_lock_ecm_msg = None
self.mtml_start_position = None
self.mtml_end_position = None
self.initialize_psms_initialized = 30
self.__DEBUG_GRAPHICS__ = False
def execute_move(self, pos):
rospy.loginfo('moving')
# Read in pose data. Adjust the height to be above the block and the length so that the laser sees the table instead of the block
pos.position.z += .1
pos.position.x += .005
q = [pos.orientation.w, pos.orientation.x, pos.orientation.y, pos.orientation.z]
p =[[pos.position.x],[pos.position.y],[pos.position.z]]
# Convert quaternion data to rotation matrix
R = quat_to_so3(q);
# Form transformation matrix
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
# Create seed with current position
q0 = kdl_kin.random_joint_angles()
limb_interface = baxter_interface.limb.Limb('right')
limb_interface.set_joint_position_speed(.3)
current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
for ind in range(len(q0)):
q0[ind] = current_angles[ind]
pose = kdl_kin.forward(q0)
pose[0:3,0:3] = R
pose[0:3,3] = p
# Solve for joint angles, iterating if no solution is found
seed = 0.3
q_ik = kdl_kin.inverse(pose, q0+seed)
while q_ik == None:
seed += 0.3
q_ik = kdl_kin.inverse(pose, q0+seed)
rospy.loginfo(q_ik)
# Calculate the joint trajectory with a quintic time scaling
q_list = JointTrajectory(q0,q_ik,1,50,5)
# Iterate through list
for q in q_list:
# Format joint angles as limb joint angle assignment
angles = limb_interface.joint_angles()
for ind, joint in enumerate(limb_interface.joint_names()):
angles[joint] = q[ind]
rospy.sleep(.07)
# Send joint move command
limb_interface.set_joint_positions(angles)
# Publish state and hand position
rospy.sleep(1)
rospy.loginfo(4)
self._pub_state.publish(4)
rospy.loginfo(pos)
self._pub_hand.publish(pos)
self._done = True
print('Done')
def __init__(self):
self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
self.psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
self.psm2_kin = KDLKinematics(self.psm2_robot, self.psm2_robot.links[0].name, self.psm2_robot.links[-1].name)
self.zoom_level_positions = {'l1':None, 'r1':None, 'l2':None, 'r2':None, 'lm':None, 'rm':None}
self.logerror("autocamera_initialized")
def move_to(pos):
pub_demo_state = rospy.Publisher('demo_state',Int16, queue_size = 10)
if (pos == 1):
catch = np.array([.65, .2, 0]) # my .7?
R = np.array([[ 0.26397895, -0.34002068, 0.90260791],
[-0.01747676, -0.93733484, -0.34799134],
[ 0.96437009, 0.07608772, -0.25337913]])
elif (pos == 2):
catch = np.array([.68, -.05, 0])
R = np.array([[0,0,1],[0,-1,0],[1,0,0]])
elif (pos == 3):
catch = np.array([.72, .1, 0])
R = np.array([[ 0.26397895, -0.34002068, 0.90260791],
[-0.01747676, -0.93733484, -0.34799134],
[ 0.96437009, 0.07608772, -0.25337913]])
else:
pass
th_init = np.array([-.3048, -.2703, -.1848, 1.908, .758, -1.234, -3.04])
X = RpToTrans(R,catch)
# Find end joint angles with IK
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'left_gripper_base')
seed = 0
q_ik = kdl_kin.inverse(X, th_init)
while q_ik == None:
seed += 0.01
q_ik = kdl_kin.inverse(X, th_init+seed)
if (seed>1):
# return False
break
q_goal = q_ik
print q_goal
limb_interface = baxter_interface.limb.Limb('left')
angles = limb_interface.joint_angles()
for ind, joint in enumerate(limb_interface.joint_names()):
angles[joint] = q_goal[ind]
limb_interface.move_to_joint_positions(angles)
# rospy.sleep(5)
print 'done'
pub_demo_state.publish(1)
def __init__(self, robot, base_link, end_link, group,
move_group_ns="move_group",
planning_scene_topic="planning_scene",
robot_ns="",
verbose=False,
kdl_kin=None,
closed_form_IK_solver = None,
joint_names=[]):
self.robot = robot
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(base_link, end_link)
if kdl_kin is None:
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
else:
self.kdl_kin = kdl_kin
self.base_link = base_link
self.joint_names = joint_names
self.end_link = end_link
self.group = group
self.robot_ns = robot_ns
self.client = actionlib.SimpleActionClient(move_group_ns, MoveGroupAction)
self.acceleration_magnification = 1
rospy.wait_for_service('compute_cartesian_path')
self.cartesian_path_plan = rospy.ServiceProxy('compute_cartesian_path',GetCartesianPath)
self.verbose = verbose
self.closed_form_IK_solver = closed_form_IK_solver
def __init__(self, sim=True):
f = file('/home/hirolab/catkin_ws/src/vspgrid/urdf/widowx2.urdf', 'r')
robot = URDF.from_xml_string(f.read()) # parsed URDF
# robot = URDF.from_parameter_server()
self.kin = KDLKinematics(robot, 'base', 'mconn')
# Selection of end-effector parameters for WidowX
# x, y, z, yaw, pitch
self.cart_from_matrix = lambda T: np.array([
T[0, 3],
T[1, 3],
T[2, 3],
math.atan2(T[1, 0], T[0, 0]),
-math.asin(T[2, 0])])
self.home = np.array([0.05, 0, 0.25, 0, 0]) # home end-effector pose
self.q = np.array([0, 0, 0, 0, 0]) # joint angles
self.dt = 0.05 # algorithm time step
self.sim = sim # true if virtual robot
def publish(eventStop):
# for arbotix
pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)
# for visualization
jointPub = rospy.Publisher(
"/joint_states", JointState, queue_size=5)
jmsg = JointState()
jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']
while not rospy.is_shutdown() and not eventStop.is_set():
jmsg.header.stamp = rospy.Time.now()
jmsg.position = self.q
if self.sim:
jointPub.publish(jmsg)
pub1.publish(self.q[0])
pub2.publish(self.q[1])
pub3.publish(self.q[2])
pub4.publish(self.q[3])
pub5.publish(self.q[4])
eventStop.wait(self.dt)
rospy.init_node("robot")
eventStop = threading.Event()
threadJPub = threading.Thread(target=publish, args=(eventStop,))
threadJPub.daemon = True
threadJPub.start() # Update joint angles in a background process
def execute_cb(self,goal):
self.moving_to_new_x_pos = False
self.moving_to_new_y_pos = False
self.stop_base_movement = False
self.max_virtual_x_vel = 0.05
self.max_virtual_y_vel = 0.05
self.commanded_virtual_x_pos = 0.0
self.commanded_virtual_y_pos = 0.0
self.commanded_virtual_x_vel = 0.0
self.commanded_virtual_y_vel = 0.0
self.virtual_x_cmd_time_sec = 0.0
self.virtual_y_cmd_time_sec = 0.0
self.virtual_x_running_time_sec = 0.0
self.virtual_y_running_time_sec = 0.0
youbot_urdf = URDF.from_parameter_server()
self.kin_with_virtual = KDLKinematics(youbot_urdf, "virtual", "gripper_palm_link")
self.kin_grasp = KDLKinematics(youbot_urdf, "base_link", "gripper_palm_link")
# Create a timer that will be used to monitor the velocity of the virtual
# joints when we need them to be positioning themselves.
self.vel_monitor_timer = rospy.Timer(rospy.Duration(0.1), self.vel_monitor_timer_callback)
self.arm_joint_pub = rospy.Publisher("arm_1/arm_controller/position_command",JointPositions,queue_size=10)
self.gripper_pub = rospy.Publisher("arm_1/gripper_controller/position_command", JointPositions, queue_size=10)
self.vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
# Give the publishers time to get setup before trying to do any actual work.
rospy.sleep(2)
# Initialize at the candle position.
self.publish_arm_joint_positions(armJointPosCandle)
rospy.sleep(2.0)
#self.publish_arm_joint_positions(armJointPos1)
#rospy.sleep(3.0)
#self.publish_arm_joint_positions(armJointPos2)
#rospy.sleep(10.0)
#self.publish_arm_joint_positions(armJointPosCandle)
#rospy.sleep(2.0)
# Go through the routine of picking up the block.
self.grasp_routine()
self._result.successOrNot=1
self._as.set_succeeded(self._result)
def find_everything_related_to_world(arm_name, xyzrpy):
global psm1_kin,psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot
if len(xyzrpy) > 2:
xyzrpy = [ tuple(xyzrpy[0:3]), tuple(xyzrpy[3:])]
psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[1].name)
Twp1 = psm1_kin.forward([])
if arm_name == 'psm2':
Tp12 = pose_converter.PoseConv.to_homo_mat(xyzrpy)
Twp2 = Twp1 * Tp12
Twp2_euler = pose_converter.PoseConv.to_pos_euler(Twp2)
return Twp2_euler
if arm_name == 'ecm':
ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[1].name, ecm_robot.links[3].name)
Tse = ecm_kin.forward([])
Tp1E = pose_converter.PoseConv.to_homo_mat(xyzrpy)
Tws = Twp1 * Tp1E * (Tse**-1)
Tws_euler = pose_converter.PoseConv.to_pos_euler(Tws)
return Tws_euler
def kdl_kinematics (self,data): self.q_sensors = data self.tree = kdl_tree_from_urdf_model(self.omni) # create a kdl tree from omni URDF model self.omni_kin = KDLKinematics(self.omni, "base", "stylus") # create instance that captures the kinematics of the robot arm #Forward Kinematics self.pose_stylus = self.omni_kin.forward(data) #compute the forward kinematics from the sensor joint angle position using the kinematics from the kdl tree #Inverse Kinematics self.q_guess = numpy.array(data)+0.2 #make an initial guess for your joint angles by deviating all the sensor joint angles by 0.2 self.q_ik = self.omni_kin.inverse(self.pose_stylus, self.q_guess) #using the position from the forward kinematics 'pose_stylus' and the offset initial guess, compute #the desired joint angles for that position. self.delta_q = self.q_ik-data
def __init__(self):
"""Read robot describtion"""
self.robot_ = URDF.from_parameter_server()
self.kdl_kin_ = KDLKinematics(self.robot_, 'base_link', 'end_effector')
self.cur_pose_ = None
self.cur_jnt_ = None
# Creates the SimpleActionClient, passing the type of the action
self.client_ = actionlib.SimpleActionClient('/mitsubishi_arm/mitsubishi_trajectory_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction)
self.client_.wait_for_server()
self.goal_ = control_msgs.msg.FollowJointTrajectoryGoal()
#time_from_start=rospy.Duration(10)
self.goal_.trajectory.joint_names=['j1','j2','j3','j4','j5','j6']
#To be reached 1 second after starting along the trajectory
trajectory_point=trajectory_msgs.msg.JointTrajectoryPoint()
trajectory_point.positions=[0]*6
trajectory_point.time_from_start=rospy.Duration(0.1)
self.goal_.trajectory.points.append(trajectory_point)
rospy.Subscriber('joint_states', sensor_msgs.msg.JointState, self.fwd_kinematics_cb)
rospy.Subscriber('command', geometry_msgs.msg.Pose, self.inv_kinematics_cb)
def __init__(self):
# For forward and inverse kinematics
self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
self.mtml_robot = URDF.from_parameter_server('/dvrk_mtml/robot_description')
self.mtml_kin = KDLKinematics(self.mtml_robot, self.mtml_robot.links[0].name, self.mtml_robot.links[-1].name)
self.mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description')
self.mtmr_kin = KDLKinematics(self.mtmr_robot, self.mtmr_robot.links[0].name, self.mtmr_robot.links[-1].name)
# For camera clutch control
self.camera_clutch_pressed = False
self.ecm_manual_control_lock_mtml_msg = None
self.ecm_manual_control_lock_ecm_msg = None
self.mtml_start_position = None
self.mtml_end_position = None
self.ecm_msg = None
self.__clutchNGo_mode__ = self.MODE.simulation
self.autocamera = Autocamera()
def main():
a = rospy.init_node('set_base_frames')
global psm1_kin, psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot, mtmr_robot, mtmr_kin
if psm1_robot is None:
psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[1].name)
if psm2_robot is None:
psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[1].name)
if ecm_robot is None:
ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name)
ecm_base = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[3].name)
if mtmr_robot is None:
mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description')
mtmr_base = KDLKinematics(mtmr_robot, mtmr_robot.links[0].name, mtmr_robot.links[1].name)
# pdb.set_trace()
psm1_pub = rospy.Publisher('/dvrk/PSM1/set_base_frame', Pose, latch=True, queue_size=1)
psm2_pub = rospy.Publisher('/dvrk/PSM2/set_base_frame', Pose, latch=True, queue_size=1)
mtmr_pub = rospy.Publisher('/dvrk/MTMR/set_base_frame', Pose, latch=True, queue_size=1)
ecm_tip = ecm_kin.forward([1,1,1,1]) # ECM Tool Tip
ecm_tip = np.linalg.inv(ecm_tip)
psm1_base_frame = psm1_kin.forward([])
psm1_message = pose_converter.PoseConv.to_pose_msg(psm1_base_frame)
psm2_base_frame = psm2_kin.forward([])
psm2_message = pose_converter.PoseConv.to_pose_msg(psm2_base_frame)
psm1_pub.publish(psm1_message)
psm2_pub.publish(psm2_message)
# mtmr_pub.publish(message)
print (psm1_message)
print (psm2_message)
sleep (1)
def find_path(plot, pos):
# Set initial position
q_start = np.array([
-0.22281071, -0.36470393, 0.36163597, 1.71920897, -0.82719914,
-1.16889336, -0.90888362
])
limb_interface = baxter_interface.limb.Limb('right')
angles = limb_interface.joint_angles()
for ind, joint in enumerate(limb_interface.joint_names()):
q_start[ind] = angles[joint]
print q_start
# Find goal for throwing
if pos == 1:
catch_y = .7
elif pos == 2:
catch_y = .1
elif pos == 3:
catch_y = .3
pos_goal = find_feasible_release(catch_x, catch_y, catch_z, pos)
block_width = .047
throw_y = pos_goal[0]
throw_z = pos_goal[1]
dy = catch_y - throw_y - block_width
vel = pos_goal[2]
alpha = pos_goal[3]
t = np.linspace(0, dy / (vel * cos(alpha)), 100)
traj_y = vel * cos(alpha) * t + throw_y
traj_z = -.5 * 9.8 * t**2 + vel * sin(alpha) * t + throw_z
if (plot == True):
plt.close('all')
plt.figure()
plt.hold(False)
plt.plot(traj_y, traj_z, 'r', linewidth=2)
plt.hold(True)
plt.plot(traj_y[0], traj_z[0], 'r.', markersize=15)
plt.ylim([-.8, .5])
plt.xlim([-.2, .8])
plt.xlabel('Y position (m)')
plt.ylabel('Z position (m)')
plt.title('Desired trajectory')
plt.show(block=False)
wm = plt.get_current_fig_manager()
wm.window.wm_geometry("800x500+1000+0")
# wm.window.setGeometry(800,500,0,0)
raw_input("Press enter to continue...")
# plt.show(block=False)
print('found release state')
print pos_goal
# Add rotation to position and convert to rotation matrix
R = np.array([[0.11121663, -0.14382586, 0.98333361],
[-0.95290138, -0.2963578, 0.06442835],
[0.28215212, -0.94418546, -0.17001177]])
p = np.hstack((0.68, pos_goal[0:2]))
X = RpToTrans(R, p)
# Find end joint angles with IK
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
thInit = q_start
seed = 0
q_ik = kdl_kin.inverse(X, thInit)
while q_ik == None:
seed += 0.01
q_ik = kdl_kin.inverse(X, thInit + seed)
if (seed > 1):
# return False
break
q_goal = q_ik
# print q_goal
# Transform to joint velocities using Jacobian
jacobian = kdl_kin.jacobian(q_ik)
inv_jac = np.linalg.pinv(jacobian)
Vb = np.array([0, 0, 0, 0, pos_goal[2], pos_goal[3]])
q_dot_goal = inv_jac.dot(Vb)
iter = 1000
treeA = np.empty((iter + 1, 14))
edgesA = np.empty((iter, 2))
treeB = np.empty((iter + 1, 14))
edgesB = np.empty((iter, 2))
treeA[0] = np.hstack((q_start, np.array([0, 0, 0, 0, 0, 0, 0])))
treeB[0] = np.hstack((q_goal, q_dot_goal.tolist()[0]))
treeA, treeB, edgesA, edgesB, indA, indB = build_tree(
iter, treeA, treeB, edgesA, edgesB, plot, kdl_kin)
np.linalg.norm(q_start - q_goal)
pathA = np.empty((edgesA.shape[0], 14))
pathA[0] = treeA[indA]
curEdge = edgesA[indA - 1]
ind = 1
atOrigin = False
while atOrigin == False:
pathA[ind] = treeA[curEdge[0]]
curEdge = edgesA[curEdge[0] - 1]
ind += 1
if curEdge[0] == 0:
atOrigin = True
pathA[ind] = treeA[curEdge[0]]
pathA = pathA[0:ind + 1, :]
pathB = np.empty((edgesB.shape[0], 14))
pathB[0] = treeB[indB]
curEdge = edgesB[indB - 1]
ind = 1
atOrigin = False
# print treeB, pathB
while atOrigin == False:
pathB[ind] = treeB[curEdge[0]]
curEdge = edgesB[curEdge[0] - 1]
if curEdge[0] == 0:
atOrigin = True
# ind += 1
else:
ind += 1
pathB[ind] = treeB[curEdge[0]]
pathB = pathB[0:ind + 1, :]
path = np.vstack((pathA[::-1], pathB))
if (plot):
stepList = np.empty((path.shape[0], 3))
for i in range(path.shape[0]):
stepList[i] = kdl_kin.forward(path[i, :7])[0:3, 3].transpose()
plt.plot(stepList[:, 1], stepList[:, 2], 'g', linewidth=2)
plt.show(block=False)
raw_input('Press enter to continue...')
plt.close('all')
# if (plot):
# plt.figure()
# plt.plot(path[:,0:7],'.')
# plt.show(block=False)
# print(path.shape)
return path
class JTCartesianController(object):
def __init__(self):
limb = read_parameter('~limb', 'right')
if limb not in ['right', 'left']:
rospy.logerr('Unknown limb name [%s]' % limb)
return
# Read the controllers parameters
gains = read_parameter('~gains', dict())
self.kp = joint_list_to_kdl(gains['Kp'])
self.kd = joint_list_to_kdl(gains['Kd'])
self.publish_rate = read_parameter('~publish_rate', 500)
self.frame_id = read_parameter('~frame_id', 'base')
self.timeout = read_parameter('~timeout', 0.02) # seconds
# Set-up baxter interface
self.arm_interface = baxter_interface.Limb(limb)
# set_joint_torques must be commanded at a rate great than the timeout specified by set_command_timeout.
# If the timeout is exceeded before a new set_joint_velocities command is received, the controller will switch
# modes back to position mode for safety purposes.
self.arm_interface.set_command_timeout(self.timeout)
# Baxter kinematics
self.urdf = URDF.from_parameter_server(key='robot_description')
self.tip_link = '%s_gripper' % limb
self.kinematics = KDLKinematics(self.urdf, self.frame_id,
self.tip_link)
self.fk_vel_solver = PyKDL.ChainFkSolverVel_recursive(
self.kinematics.chain)
# Adjust the publish rate of baxter's state
joint_state_pub_rate = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
# Get the joint names and limits
self.joint_names = self.arm_interface.joint_names()
self.num_joints = len(self.joint_names)
self.lower_limits = np.zeros(self.num_joints)
self.upper_limits = np.zeros(self.num_joints)
# KDL joint may be in a different order than expected
kdl_lower_limits, kdl_upper_limits = self.kinematics.get_joint_limits()
for i, name in enumerate(self.kinematics.get_joint_names()):
if name in self.joint_names:
idx = self.joint_names.index(name)
self.lower_limits[idx] = kdl_lower_limits[i]
self.upper_limits[idx] = kdl_upper_limits[i]
self.middle_values = (self.upper_limits + self.lower_limits) / 2.0
# Move the arm to a neutral position
#~ self.arm_interface.move_to_neutral(timeout=10.0)
#~ neutral_pos = [-math.pi/4, 0.0, 0.0, 0.0, 0.0, math.pi/2, -math.pi/2]
neutral_pos = [1.0, -0.57, -1.2, 1.3, 0.86, 1.4, -0.776]
#~ neutral_pos = [pi/4, -pi/3, 0.0, pi/2, 0.0, pi/3, 0.0]
neutral_cmd = dict()
for i, name in enumerate(self.joint_names):
if name in ['left_s0', 'left_w2']:
neutral_cmd[name] = neutral_pos[i] * -1.0
else:
neutral_cmd[name] = neutral_pos[i]
self.arm_interface.move_to_joint_positions(neutral_cmd, timeout=10.0)
# Baxter faces
self.limb = limb
self.face = FaceImage()
self.face.set_image('happy')
# Set-up publishers/subscribers
self.suppress_grav_comp = rospy.Publisher(
'/robot/limb/%s/suppress_gravity_compensation' % limb, Empty)
joint_state_pub_rate.publish(int(self.publish_rate))
self.feedback_pub = rospy.Publisher('/takktile/force_feedback', Wrench)
rospy.Subscriber('/baxter/%s_ik_command' % limb, PoseStamped,
self.ik_command_cb)
self.gripper_closed = False
rospy.Subscriber('/takktile/calibrated', Touch, self.takktile_cb)
rospy.Subscriber('/robot/end_effector/%s_gripper/state' % limb,
EndEffectorState, self.end_effector_cb)
rospy.loginfo('Running Cartesian controller for the %s arm' % limb)
# Start torque controller timer
self.cart_command = None
self.torque_controller_timer = rospy.Timer(
rospy.Duration(1.0 / self.publish_rate), self.torque_controller_cb)
# Shutdown hookup to clean-up before killing the script
rospy.on_shutdown(self.shutdown)
def end_effector_cb(self, msg):
self.gripper_closed = msg.position < 80.0
def get_joint_angles_array(self):
out = np.zeros(self.num_joints)
joint_angles = self.arm_interface.joint_angles()
for i, name in enumerate(self.joint_names):
out[i] = joint_angles[name]
return out
def ik_command_cb(self, msg):
if self.cart_command == None:
self.face.set_image('thinking_%s' % self.limb)
self.cart_command = msg
# Add stamp because the console app doesn't use it
self.cart_command.header.stamp = rospy.Time.now()
def shutdown(self):
self.face.set_image('indifferent')
# This order mathers!
self.arm_interface.exit_control_mode()
self.torque_controller_timer.shutdown()
def takktile_cb(self, msg):
if self.gripper_closed:
y = 0
else:
y = 0.003 * (sum(msg.pressure[0:4]) - sum(msg.pressure[6:10]))
z = 0.006 * (msg.pressure[4] + msg.pressure[10])
# Changes the reference frame of the force vector
T = baxter_to_kdl_frame(self.arm_interface.endpoint_pose())
force = T.M * PyKDL.Vector(0, y, z)
# Populate the wrench message
wrench = Wrench()
wrench.force.x = force.x()
wrench.force.y = force.y()
wrench.force.z = force.z()
try:
self.feedback_pub.publish(wrench)
except:
pass
def torque_controller_cb(self, event):
if rospy.is_shutdown() or self.cart_command == None:
return
elapsed_time = rospy.Time.now() - self.cart_command.header.stamp
if elapsed_time.to_sec() > self.timeout:
return
# TODO: Validate msg.header.frame_id
## Cartesian error to zero using a Jacobian transpose controller
x_target = posemath.fromMsg(self.cart_command.pose)
q = self.get_joint_angles_array()
x = baxter_to_kdl_frame(self.arm_interface.endpoint_pose())
xdot = baxter_to_kdl_twist(self.arm_interface.endpoint_velocity())
# Calculate a Cartesian restoring wrench
x_error = PyKDL.diff(x_target, x)
wrench = np.matrix(np.zeros(6)).T
for i in range(len(wrench)):
wrench[i] = -(self.kp[i] * x_error[i] + self.kd[i] * xdot[i])
# Calculate the jacobian
J = self.kinematics.jacobian(q)
# Convert the force into a set of joint torques. tau = J^T * wrench
tau = J.T * wrench
# Populate the joint_torques in baxter API format (dictionary)
joint_torques = dict()
for i, name in enumerate(self.joint_names):
joint_torques[name] = tau[i]
self.arm_interface.set_joint_torques(joint_torques)
def __init__(self, hebi_group_name, hebi_mapping, hebi_gains, urdf_str, base_link, end_link):
rospy.loginfo("Creating {} instance".format(self.__class__.__name__))
self.openmeta_testbench_manifest_path = rospy.get_param('~openmeta/testbench_manifest_path', None)
if self.openmeta_testbench_manifest_path is not None:
self._testbench_manifest = load_json_file(self.openmeta_testbench_manifest_path)
# TestBench Parameters
self._params = {}
for tb_param in self._testbench_manifest['Parameters']:
self._params[tb_param['Name']] = tb_param['Value'] # WARNING: If you use these values - make sure to check the type
# TestBench Metrics
self._metrics = {}
for tb_metric in self._testbench_manifest['Metrics']: # FIXME: Hmm, this is starting to look a lot like OpenMDAO...
self._metrics[tb_metric['Name']] = tb_metric['Value']
if hebi_gains is None:
hebi_gains = {
'p': [float(self._params['p_gain'])]*3,
'i': [float(self._params['i_gain'])]*3,
'd': [float(self._params['d_gain'])]*3
}
hebi_families = []
hebi_names = []
for actuator in hebi_mapping:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_wrap = HebirosWrapper(hebi_group_name, hebi_families, hebi_names)
# Set PID Gains
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping
cmd_msg.settings.name = hebi_mapping
cmd_msg.settings.control_strategy = [4, 4, 4]
cmd_msg.settings.position_gains.name = hebi_mapping
cmd_msg.settings.position_gains.kp = hebi_gains['p']
cmd_msg.settings.position_gains.ki = hebi_gains['i']
cmd_msg.settings.position_gains.kd = hebi_gains['d']
cmd_msg.settings.position_gains.i_clamp = [0.25, 0.25, 0.25] # TODO: Tune me.
self.hebi_wrap.send_command_with_acknowledgement(cmd_msg)
if base_link is None or end_link is None:
robot = Robot.from_xml_string(urdf_str)
if not base_link:
base_link = robot.get_root()
# WARNING: There may be multiple leaf nodes
if not end_link:
end_link = [x for x in robot.link_map.keys() if x not in robot.child_map.keys()][0]
# pykdl
self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link, end_link)
self._active_joints = self.kdl_fk.get_joint_names()
# joint data
self.position_fbk = [0.0]*self.hebi_wrap.hebi_count
self.velocity_fbk = [0.0]*self.hebi_wrap.hebi_count
self.effort_fbk = [0.0]*self.hebi_wrap.hebi_count
# joint state publisher
while not rospy.is_shutdown() and len(self.hebi_wrap.get_joint_positions()) < len(self.hebi_wrap.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
# Set up Waypoint/Trajectory objects
self.start_wp = WaypointMsg()
self.start_wp.names = self.hebi_wrap.hebi_mapping
self.end_wp = copy.deepcopy(self.start_wp)
self.goal = TrajectoryGoal()
self.goal.waypoints = [self.start_wp, self.end_wp]
self._hold_positions = [0.0]*3
self._hold = True
self.jointstate = JointState()
self.jointstate.name = self.hebi_wrap.hebi_mapping
rospy.sleep(1.0)
def __init__(self,
base_link,
end_link,
planning_group,
world="/world",
listener=None,
broadcaster=None,
traj_step_t=0.1,
max_acc=1,
max_vel=1,
max_goal_diff=0.02,
goal_rotation_weight=0.01,
max_q_diff=1e-6,
start_js_cb=True,
base_steps=10,
steps_per_meter=100,
closed_form_IK_solver=None,
dof=7,
perception_ns="/SPServer"):
self.world = world
self.base_link = base_link
self.end_link = end_link
self.planning_group = planning_group
self.dof = dof
self.base_steps = base_steps
self.steps_per_meter = steps_per_meter
self.MAX_ACC = max_acc
self.MAX_VEL = max_vel
self.traj_step_t = traj_step_t
self.max_goal_diff = max_goal_diff
self.max_q_diff = max_q_diff
self.goal_rotation_weight = goal_rotation_weight
self.at_goal = True
self.near_goal = True
self.moving = False
self.q0 = None #[0] * self.dof
self.old_q0 = [0] * self.dof
self.cur_stamp = 0
# Set up TF broadcaster
if not broadcaster is None:
self.broadcaster = broadcaster
else:
self.broadcaster = tf.TransformBroadcaster()
# Set up TF listener and smartmove manager
if listener is None:
self.listener = tf.TransformListener()
else:
self.listener = listener
# Currently this class does not need a smart waypoint manager.
# That will remain in the CoSTAR BT.
#self.smartmove_manager = SmartWaypointManager(
# listener=self.listener,
# broadcaster=self.broadcaster)
# TODO: ensure the manager is set up properly
# Note that while the waypoint manager is currently a part of CostarArm
# If we wanted to set this up for multiple robots it should be treated
# as an independent component.
self.waypoint_manager = WaypointManager(service=True,
broadcaster=self.broadcaster)
# Set up services
# The CostarArm services let the UI put it into teach mode or anything else
self.teach_mode = rospy.Service('/costar/SetTeachMode', SetTeachMode,
self.set_teach_mode_call)
self.servo_mode = rospy.Service('/costar/SetServoMode', SetServoMode,
self.set_servo_mode_call)
self.shutdown = rospy.Service('/costar/ShutdownArm', EmptyService,
self.shutdown_arm_call)
self.servo = rospy.Service('/costar/ServoToPose', ServoToPose,
self.servo_to_pose_call)
self.plan = rospy.Service('/costar/PlanToPose', ServoToPose,
self.plan_to_pose_call)
self.smartmove = rospy.Service('/costar/SmartMove', SmartMove,
self.smart_move_call)
self.js_servo = rospy.Service('/costar/ServoToJointState',
ServoToJointState,
self.servo_to_joints_call)
self.save_frame = rospy.Service('/costar/SaveFrame', SaveFrame,
self.save_frame_call)
self.save_joints = rospy.Service('/costar/SaveJointPosition',
SaveFrame, self.save_joints_call)
self.get_waypoints_srv = GetWaypointsService(world=world,
service=False,
ns=perception_ns)
self.driver_status = 'IDLE'
# Create publishers. These will send necessary information out about the state of the robot.
self.pt_publisher = rospy.Publisher('/joint_traj_pt_cmd',
JointTrajectoryPoint,
queue_size=1000)
self.status_publisher = rospy.Publisher('/costar/DriverStatus',
String,
queue_size=1000)
self.display_pub = rospy.Publisher('costar/display_trajectory',
DisplayTrajectory,
queue_size=1000)
self.robot = URDF.from_parameter_server()
if start_js_cb:
self.js_subscriber = rospy.Subscriber('joint_states', JointState,
self.js_cb)
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(base_link, end_link)
# cCreate reference to pyKDL kinematics
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
#self.set_goal(self.q0)
self.goal = None
self.ee_pose = None
self.joint_names = [
joint.name for joint in self.robot.joints[:self.dof]
]
self.planner = SimplePlanning(
self.robot,
base_link,
end_link,
self.planning_group,
kdl_kin=self.kdl_kin,
joint_names=self.joint_names,
closed_form_IK_solver=closed_form_IK_solver)
class URDriver():
MAX_ACC = .5
MAX_VEL = 1.8
JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
def __init__(self):
rospy.init_node('ur_simulation',anonymous=True)
rospy.logwarn('SIMPLE_UR SIMULATION DRIVER LOADING')
# TF
self.broadcaster_ = tf.TransformBroadcaster()
self.listener_ = tf.TransformListener()
# SERVICES
self.servo_to_pose_service = rospy.Service('simple_ur_msgs/ServoToPose', ServoToPose, self.servo_to_pose_call)
self.set_stop_service = rospy.Service('simple_ur_msgs/SetStop', SetStop, self.set_stop_call)
self.set_teach_mode_service = rospy.Service('simple_ur_msgs/SetTeachMode', SetTeachMode, self.set_teach_mode_call)
self.set_servo_mode_service = rospy.Service('simple_ur_msgs/SetServoMode', SetServoMode, self.set_servo_mode_call)
# PUBLISHERS AND SUBSCRIBERS
self.driver_status_publisher = rospy.Publisher('/ur_robot/driver_status',String)
self.robot_state_publisher = rospy.Publisher('/ur_robot/robot_state',String)
self.joint_state_publisher = rospy.Publisher('joint_states',JointState)
# self.follow_pose_subscriber = rospy.Subscriber('/ur_robot/follow_goal',PoseStamped,self.follow_goal_cb)
# Predicator
self.pub_list = rospy.Publisher('/predicator/input', PredicateList)
self.pub_valid = rospy.Publisher('/predicator/valid_input', ValidPredicates)
rospy.sleep(1)
pval = ValidPredicates()
pval.pheader.source = rospy.get_name()
pval.predicates = ['moving', 'stopped', 'running']
pval.assignments = ['robot']
self.pub_valid.publish(pval)
# Rate
self.run_rate = rospy.Rate(100)
self.run_rate.sleep()
### Set Up Simulated Robot ###
self.driver_status = 'IDLE'
self.robot_state = 'POWER OFF'
robot = URDF.from_parameter_server()
self.kdl_kin = KDLKinematics(robot, 'base_link', 'ee_link')
# self.q = self.kdl_kin.random_joint_angles()
self.q = [-1.5707,-.785,-3.1415+.785,-1.5707-.785,-1.5707,0] # Start Pose?
self.start_pose = self.kdl_kin.forward(self.q)
self.F_start = tf_c.fromMatrix(self.start_pose)
# rospy.logwarn(self.start_pose)
# rospy.logwarn(type(self.start_pose))
# pose = self.kdl_kin.forward(q)
# joint_positions = self.kdl_kin.inverse(pose, q+0.3) # inverse kinematics
# if joint_positions is not None:
# pose_sol = self.kdl_kin.forward(joint_positions) # should equal pose
# J = self.kdl_kin.jacobian(q)
# rospy.logwarn('q:'+str(q))
# rospy.logwarn('joint_positions:'+str(joint_positions))
# rospy.logwarn('pose:'+str(pose))
# if joint_positions is not None:
# rospy.logwarn('pose_sol:'+str(pose_sol))
# rospy.logwarn('J:'+str(J))
### START LOOP ###
while not rospy.is_shutdown():
if self.driver_status == 'TEACH':
self.update_from_marker()
# if self.driver_status == 'SERVO':
# self.update_follow()
# Publish and Sleep
self.publish_status()
self.send_command()
self.run_rate.sleep()
# Finish
rospy.logwarn('SIMPLE UR - Simulation Finished')
def update_from_marker(self):
try:
F_target_world = tf_c.fromTf(self.listener_.lookupTransform('/world','/endpoint_interact',rospy.Time(0)))
F_target_base = tf_c.fromTf(self.listener_.lookupTransform('/base_link','/endpoint_interact',rospy.Time(0)))
F_base_world = tf_c.fromTf(self.listener_.lookupTransform('/world','/base_link',rospy.Time(0)))
self.F_command = F_base_world.Inverse()*F_target_world
M_command = tf_c.toMatrix(self.F_command)
joint_positions = self.kdl_kin.inverse(M_command, self.q) # inverse kinematics
if joint_positions is not None:
pose_sol = self.kdl_kin.forward(joint_positions) # should equal pose
self.q = joint_positions
else:
rospy.logwarn('no solution found')
# self.send_command()
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.logwarn(str(e))
def send_command(self):
self.current_joint_positions = self.q
msg = JointState()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "simuated_data"
msg.name = self.JOINT_NAMES
msg.position = self.current_joint_positions
msg.velocity = [0]*6
msg.effort = [0]*6
self.joint_state_publisher.publish(msg)
pose = self.kdl_kin.forward(self.q)
F = tf_c.fromMatrix(pose)
# F = self.F_command
self.current_tcp_pose = tf_c.toMsg(F)
self.current_tcp_frame = F
self.broadcaster_.sendTransform(tuple(F.p),tuple(F.M.GetQuaternion()),rospy.Time.now(), '/endpoint','/base_link')
def set_teach_mode_call(self,req):
if req.enable == True:
# self.rob.set_freedrive(True)
self.driver_status = 'TEACH'
return 'SUCCESS - teach mode enabled'
else:
# self.rob.set_freedrive(False)
self.driver_status = 'IDLE'
return 'SUCCESS - teach mode disabled'
def set_servo_mode_call(self,req):
if req.mode == 'SERVO':
self.driver_status = 'SERVO'
return 'SUCCESS - servo mode enabled'
elif req.mode == 'DISABLE':
self.driver_status = 'IDLE'
return 'SUCCESS - servo mode disabled'
def set_stop_call(self,req):
rospy.logwarn('SIMPLE UR - STOPPING ROBOT')
self.rob.stop()
return 'SUCCESS - stopped robot'
def servo_to_pose_call(self,req):
if self.driver_status == 'SERVO':
rospy.logwarn(req)
self.F_command = tf_c.fromMsg(req.target)
M_command = tf_c.toMatrix(self.F_command)
# Calculate IK
joint_positions = self.kdl_kin.inverse(M_command, self.q) # inverse kinematics
if joint_positions is not None:
pose_sol = self.kdl_kin.forward(joint_positions) # should equal pose
self.q = joint_positions
else:
rospy.logwarn('no solution found')
# self.send_command(F_command)
return 'SUCCESS - moved to pose'
else:
rospy.logerr('SIMPLE UR -- Not in servo mode')
return 'FAILED - not in servo mode'
def publish_status(self):
self.driver_status_publisher.publish(String(self.driver_status))
self.robot_state_publisher.publish(String(self.robot_state))
ps = PredicateList()
ps.pheader.source = rospy.get_name()
ps.statements = []
statement = PredicateStatement( predicate='moving',
confidence=1,
value=PredicateStatement.FALSE,
num_params=1,
params=['robot', '', ''])
ps.statements += [statement]
statement = PredicateStatement( predicate='stopped',
confidence=1,
value=PredicateStatement.TRUE,
num_params=1,
params=['robot', '', ''])
ps.statements += [statement]
statement = PredicateStatement( predicate='running',
confidence=1,
value=PredicateStatement.TRUE,
num_params=1,
params=['robot', '', ''])
ps.statements += [statement]
self.pub_list.publish(ps)
def check_robot_state(self):
self.robot_state == 'RUNNING SIMULATION'
self.driver_status = 'IDLE - SIMULATION'
def execute_move(self, pos):
# Close the gripper
self._right_gripper.close()
self._right_gripper.close()
self._right_gripper.close()
self._right_gripper.close()
self._right_gripper.close()
rospy.loginfo('moving')
# Get robot parameters and create a limb interface instance
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
limb_interface = baxter_interface.limb.Limb('right')
angles = limb_interface.joint_angles()
limb_interface.set_joint_position_speed(.7)
q_goal = [-.01859, -.5119, 1.7909, 1.232, -1.030, 1.945, -1.31]
q0 = kdl_kin.random_joint_angles()
current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
for ind in range(len(q0)):
q0[ind] = current_angles[ind]
q_list = JointTrajectory(q0,np.asarray(q_goal),1,50,5)
for q in q_list:
for ind, joint in enumerate(limb_interface.joint_names()):
angles[joint] = q[ind]
limb_interface.set_joint_positions(angles)
rospy.sleep(.03)
# Create the desired joint trajectory with a quintic time scaling
q0 = q_goal
# Set the dropoff position to be dropoff #1
if self._goal == 1:
q_goal = [-.675, -.445, 1.626, 1.1336, -1.457, 1.6145, -2.190]
# Dropoff #2
else:
q_goal = [-.066, -.068, 1.738, .8022, -2.23, .917, -2.9057]
q_list = JointTrajectory(np.asarray(q0),np.asarray(q_goal),1,50,5)
for q in q_list:
for ind, joint in enumerate(limb_interface.joint_names()):
angles[joint] = q[ind]
limb_interface.set_joint_positions(angles)
rospy.sleep(.03)
# Open the gripper
rospy.sleep(.2)
self._right_gripper.open()
self._right_gripper.open()
self._right_gripper.open()
self._right_gripper.open()
self._right_gripper.open()
# Set the position to the old goal and the new goal to the desired camera position for seeing the blocks
q0 = q_goal
q_goal = [-.01859, -.5119, 1.7909, 1.232, -1.030, 1.945, -1.31]
q_list = JointTrajectory(np.asarray(q0),np.asarray(q_goal),1,50,5)
for q in q_list:
for ind, joint in enumerate(limb_interface.joint_names()):
angles[joint] = q[ind]
limb_interface.set_joint_positions(angles)
rospy.sleep(.05)
rospy.sleep(.2)
# Publish next state
self._pub_state.publish(1)
self._done = True
print('Done')
from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model from pykdl_utils.kdl_kinematics import KDLKinematics # VERSION CHANGE - http://answers.ros.org/question/197609/how-to-read-a-urdf-in-python-in-indigo/ #robot = URDF.load_from_parameter_server(verbose=False) robot = URDF.from_parameter_server() base_link = robot.get_root() end_link = "link_6" #robot.link_map.keys()[len(robot.link_map)-1] # robot.links[6] print end_link tree = kdl_tree_from_urdf_model(robot) print tree.getNrOfSegments() chain = tree.getChain(base_link, end_link) print chain.getNrOfJoints() kdl_kin = KDLKinematics(robot, base_link, end_link) q = kdl_kin.random_joint_angles() print 'joints:', q pose = kdl_kin.forward(q) # forward kinematics (returns homogeneous 4x4 numpy.mat) print 'pose:', pose #q_ik = kdl_kin.inverse(pose, q+0.3) # inverse kinematics #if q_ik is not None: # pose_sol = kdl_kin.forward(q_ik) # should equal pose #J = kdl_kin.jacobian(q) #print 'q:', q #print 'q_ik:', q_ik #if q_ik is not None: # print 'pose_sol:', pose_sol #print 'J:', J
class JTCartesianController(object):
def __init__(self):
# Read the controllers parameters
gains = read_parameter('~gains', dict())
self.kp = joint_list_to_kdl(gains['Kp'])
self.kd = joint_list_to_kdl(gains['Kd'])
self.publish_rate = read_parameter('~publish_rate', 500)
self.frame_id = read_parameter('~frame_id', 'base_link')
self.tip_link = read_parameter('~tip_link', 'end_effector')
# Kinematics
self.urdf = URDF.from_parameter_server(key='robot_description')
self.kinematics = KDLKinematics(self.urdf, self.frame_id,
self.tip_link)
# Get the joint names and limits
self.joint_names = self.kinematics.get_joint_names()
self.num_joints = len(self.joint_names)
# Set-up publishers/subscribers
self.torque_pub = dict()
for i, name in enumerate(self.joint_names):
self.torque_pub[name] = rospy.Publisher('/%s/command' % (name),
Float64)
rospy.Subscriber('/joint_states', JointState, self.joint_states_cb)
rospy.Subscriber('/grips/endpoint_state', EndpointState,
self.endpoint_state_cb)
rospy.Subscriber('/grips/ik_command', PoseStamped, self.ik_command_cb)
rospy.loginfo('Running Cartesian controller for Grips')
# Start torque controller timer
self.cart_command = None
self.endpoint_state = None
self.joint_states = None
self.torque_controller_timer = rospy.Timer(
rospy.Duration(1.0 / self.publish_rate), self.torque_controller_cb)
# Shutdown hookup to clean-up before killing the script
rospy.on_shutdown(self.shutdown)
def endpoint_state_cb(self, msg):
self.endpoint_state = msg
def ik_command_cb(self, msg):
self.cart_command = msg
# Add stamp manually. This is a hack for console commands (rostopic pub)
self.cart_command.header.stamp = rospy.Time.now()
def joint_states_cb(self, msg):
self.joint_states = msg
def shutdown(self):
self.torque_controller_timer.shutdown()
# Stop the torque commands. It avoids unwanted movements after stoping this controller
for i, name in enumerate(self.joint_names):
self.torque_pub[name].publish(0.0)
def torque_controller_cb(self, event):
if rospy.is_shutdown() or None in [
self.cart_command, self.endpoint_state, self.joint_states
]:
return
# TODO: Validate msg.header.frame_id
## Cartesian error to zero using a Jacobian transpose controller
x_target = posemath.fromMsg(self.cart_command.pose)
q, qd, eff = self.kinematics.extract_joint_state(self.joint_states)
x = posemath.fromMsg(self.endpoint_state.pose)
xdot = TwistMsgToKDL(self.endpoint_state.twist)
# Calculate a Cartesian restoring wrench
x_error = PyKDL.diff(x_target, x)
wrench = np.matrix(np.zeros(6)).T
for i in range(len(wrench)):
wrench[i] = -(self.kp[i] * x_error[i] + self.kd[i] * xdot[i])
# Calculate the jacobian
J = self.kinematics.jacobian(q)
# Convert the force into a set of joint torques. tau = J^T * wrench
tau = J.T * wrench
# Publish the joint_torques
for i, name in enumerate(self.joint_names):
self.torque_pub[name].publish(tau[i])
from tf.transformations import quaternion_from_euler
import tf, time
import tf.transformations as tfm
from urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_kinematics import KDLKinematics
import helper
from helper import util
import rospy
import moveit_msgs
from moveit_msgs.srv import GetPositionIK
from moveit_msgs.srv import GetPositionIKRequest
from moveit_msgs.srv import GetPositionIKResponse
#define forward kinematics configuration
robot = URDF.from_parameter_server()
fk_solver_r = KDLKinematics(robot, "yumi_body", "yumi_tip_r")
fk_solver_l = KDLKinematics(robot, "yumi_body", "yumi_tip_l")
# fk_solver_r = KDLKinematics(robot, "yumi_body", "yumi_joint_6_r")
# fk_solver_l = KDLKinematics(robot, "yumi_body", "yumi_joint_6_l")
#define inverse kinematics configuration
ik_solver_r = IK("yumi_body", "yumi_tip_r")
ik_solver_l = IK("yumi_body", "yumi_tip_l")
ik_srv = rospy.ServiceProxy('/compute_ik', GetPositionIK)
def joints_from_pose_list(pose_list, seed_both_arms=None):
arm_list = ['l', 'r']
dict_ik_pose = {}
dict_ik_pose['joints'] = []
class Push(State):
def __init__(self, hebiros_wrapper, urdf_str, base_link, end_link):
"""SMACH State
:type hebiros_wrapper: HebirosWrapper
:param hebiros_wrapper: HebirosWrapper instance for Leg HEBI group
:type urdf_str: str
:param urdf_str: Serialized URDF str
:type base_link: str
:param base_link: Leg base link name in URDF
:type end_link: str
:param end_link: Leg end link name in URDF
"""
State.__init__(self,
outcomes=['ik_failed', 'success'],
input_keys=[
'prev_joint_pos', 'target_end_link_point',
'execution_time'
],
output_keys=['prev_joint_pos', 'active_joints'])
self.hebi_wrap = hebiros_wrapper
self.urdf_str = urdf_str
self.base_link = base_link
self.end_link = end_link
self.active = False
# hardware interface
self._hold_leg_position = True
self._hold_joint_angles = []
self.hebi_wrap.add_feedback_callback(self._hold_leg_pos_cb)
# pykdl
self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link,
end_link)
self._active_joints = self.kdl_fk.get_joint_names()
# trac-ik
self.trac_ik = IK(base_link,
end_link,
urdf_string=urdf_str,
timeout=0.01,
epsilon=1e-4,
solve_type="Distance")
self.ik_pos_xyz_bounds = [0.01, 0.01, 0.01]
self.ik_pos_wxyz_bounds = [31416.0, 31416.0, 31416.0
] # NOTE: This implements position-only IK
# joint state publisher
while not rospy.is_shutdown() and len(
self.hebi_wrap.get_joint_positions()) < len(
self.hebi_wrap.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=1)
self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
def enter(self, ud):
self._hold_joint_angles = ud.prev_joint_pos
self.active = True
def execute(self, ud):
self.enter(ud)
init_wp = WaypointMsg()
end_wp = WaypointMsg()
eff_prev_target_pos = self.kdl_fk.forward(
ud.prev_joint_pos)[:3, 3].reshape(1, 3).tolist()[0]
jt_init_pos = self.hebi_wrap.get_joint_positions()
eff_init_pos = self.kdl_fk.forward(jt_init_pos)[:3, 3].reshape(
1, 3).tolist()[0]
eff_target_pos = [
ud.target_end_link_point.x, ud.target_end_link_point.y,
ud.target_end_link_point.z
]
# init_wp
init_wp.names = self.hebi_wrap.hebi_mapping
init_wp.positions = jt_init_pos
init_wp.velocities = [0.0] * self.hebi_wrap.hebi_count
init_wp.accelerations = [0.0] * self.hebi_wrap.hebi_count
# end_wp
end_wp.names = self.hebi_wrap.hebi_mapping
success, end_wp.positions = self._get_pos_ik(
self.trac_ik,
init_wp.positions,
eff_target_pos,
seed_xyz=eff_prev_target_pos)
end_wp.velocities = [0.0] * self.hebi_wrap.hebi_count
end_wp.accelerations = [0.0] * self.hebi_wrap.hebi_count
goal = TrajectoryGoal()
goal.waypoints = [init_wp, end_wp]
goal.times = [0.0, ud.execution_time]
# send goal to trajectory action server
self._hold_leg_position = False
self.hebi_wrap.trajectory_action_client.send_goal(goal)
self.hebi_wrap.trajectory_action_client.wait_for_result()
self._hold_leg_position = True
ud.prev_joint_pos = end_wp.positions
self.exit(ud)
return 'success'
def exit(self, ud):
ud.active_joints = self._active_joints
self.active = False
def _get_pos_ik(self,
ik_solver,
seed_angles,
target_xyz,
target_wxyz=None,
seed_xyz=None,
recursion_depth_cnt=100):
if recursion_depth_cnt < 0:
rospy.logdebug("%s FAILURE. Maximum recursion depth reached",
self._get_pos_ik.__name__)
return False, seed_angles
if target_wxyz is None:
target_wxyz = [
1, 0, 0, 0
] # trak-ik seems a little more stable when given initial pose for pos-only ik
target_jt_angles = ik_solver.get_ik(
seed_angles, target_xyz[0], target_xyz[1], target_xyz[2],
target_wxyz[0], target_wxyz[1], target_wxyz[2], target_wxyz[3],
self.ik_pos_xyz_bounds[0], self.ik_pos_xyz_bounds[1],
self.ik_pos_xyz_bounds[2], self.ik_pos_wxyz_bounds[0],
self.ik_pos_wxyz_bounds[1], self.ik_pos_wxyz_bounds[2])
if target_jt_angles is not None:
rospy.logdebug(
"%s SUCCESS. Solution: %s to target xyz: %s from seed angles: %s",
self._get_pos_ik.__name__, round_list(target_jt_angles, 4),
round_list(target_xyz, 4), round_list(seed_angles, 4))
return True, target_jt_angles
else: # ik_solver failed
if seed_xyz is None:
return False, seed_angles
else: # binary recursive search
target_xyz_new = [(a + b) / 2.0
for a, b in zip(target_xyz, seed_xyz)]
success, recursive_jt_angles = self._get_pos_ik(
ik_solver, seed_angles, target_xyz_new, target_wxyz,
seed_xyz, recursion_depth_cnt - 1)
if not success:
return False, seed_angles
else:
return self._get_pos_ik(ik_solver, recursive_jt_angles,
target_xyz, target_wxyz,
target_xyz_new,
recursion_depth_cnt - 1)
def _hold_leg_pos_cb(self, msg):
if not rospy.is_shutdown() and self.active and self._hold_leg_position:
jointstate = JointState()
jointstate.name = self.hebi_wrap.hebi_mapping
jointstate.position = self._hold_joint_angles
jointstate.velocity = []
jointstate.effort = []
self.hebi_wrap.joint_state_publisher.publish(jointstate)
def _joint_state_cb(self, msg):
if not rospy.is_shutdown() and self.active:
jointstate = JointState()
jointstate.header.stamp = rospy.Time.now()
jointstate.name = self._active_joints
jointstate.position = self.hebi_wrap.get_joint_positions()
jointstate.velocity = []
jointstate.effort = []
self._joint_state_pub.publish(jointstate)
class dxf_robot_motion:
def __init__(self):
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.seam_point_dict = {}
self.plan_seam_dict = {}
self.speed = 1.0
self.display = True
robot_urdf = URDF.from_parameter_server()
#robot_urdf = URDF.from_xml_string(urdf_str)
self.kdl_kin = KDLKinematics(robot_urdf, "base_link", "tool0")
self.move_group = moveit_commander.MoveGroupCommander("GP12_Planner")
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
planning_frame = self.move_group.get_planning_frame()
print("============ Planning frame: %s" % planning_frame)
self.dxf_file_response_pub = rospy.Publisher('file_response_string',
std_msgs.msg.String,
queue_size=10)
self.plan_response_pub = rospy.Publisher('plan_response_string',
std_msgs.msg.String,
queue_size=10)
self.execute_seam_response_pub = rospy.Publisher(
'execute_seam_response_string', std_msgs.msg.String, queue_size=10)
self.plan_and_execute_response_pub = rospy.Publisher(
'plan_and_execute_seam_response_string',
std_msgs.msg.String,
queue_size=10)
self.move_to_stream_start_response_pub = rospy.Publisher(
'move_to_seam_response', std_msgs.msg.String, queue_size=10)
#Ros message used to pass dxf file name to node and process dxf file
rospy.Subscriber("dxf_file_name", String, self.dxf_grabber_readfile)
#Ros message used to plan and view motion before returning
rospy.Subscriber("plan_seam_motion", String,
self.plan_robot_motion_call)
rospy.Subscriber("execute_seam_motion", String,
self.execute_robot_motion_call)
rospy.Subscriber("plan_and_execute_seam", String,
self.plan_and_execute_call)
rospy.Subscriber("move_to_seam_start", String,
self.move_to_seam_start_call)
self.followjointaction = rospy.Publisher(
"joint_trajectory_action/goal",
control_msgs.msg.FollowJointTrajectoryActionGoal,
queue_size=10)
def move_to_seam_start_call(self, data):
if (self.seam_point_dict.has_key(data.data)):
motion_point = self.seam_point_dict[data.data][0][0]
sine_val = self.seam_point_dict[data.data][1][0]
self.move_to_seam_start(motion_point, sine_val)
#print("finished_move")
self.move_to_stream_start_response_pub.publish(
"Moved to start of seam")
print("finished_move")
else:
self.move_to_stream_start_response_pub.publish(
"Seam key does not exist in loaded dictionary")
def plan_robot_motion_call(self, data):
if (self.seam_point_dict.has_key(data.data)):
motion_points = self.seam_point_dict[data.data][0]
sine_vals = self.seam_point_dict[data.data][1]
self.display = True
plan, fraction, waypoints = self.plan_robot_motion(
motion_points, sine_vals)
if (fraction == 0.0):
self.plan_response_pub.publish("Failed to plan")
else:
self.plan_seam_dict[data.data] = plan
self.plan_response_pub.publish("Planning successful")
else:
self.plan_response_pub.publish(
"Planned seam key does not exist in loaded dictionary")
def execute_robot_motion_call(self, data):
if (self.plan_seam_dict.has_key(data.data)):
plan = self.plan_seam_dict[data.data]
self.execute_planned_trajectory(plan)
self.execute_seam_response_pub.publish("Execution successful")
else:
self.execute_seam_response_pub.publish(
"Could not find plan in dictionary of planned seams, did you publish to plan_seam_motion first?"
)
def plan_and_execute_call(self, data):
if (self.seam_point_dict.has_key(data.data)):
motion_points = self.seam_point_dict[data.data][0]
sine_vals = self.seam_point_dict[data.data][1]
self.display = False
plan, fraction, waypoints = self.plan_robot_motion(
motion_points, sine_vals)
self.execute_planned_trajectory(plan)
self.plan_and_execute_response_pub.publish(
"Planning and execution successful")
else:
self.plan_and_execute_response_pub.publish(
"Planned seam key does not exist in loaded dictionary")
def distance_calculator(self, start, end):
return math.sqrt((start[0] - end[0])**2 + (start[1] - end[1])**2)
def display_robot_trajectory(self, plan):
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state()
display_trajectory.trajectory.append(plan)
self.display_trajectory_publisher.publish(display_trajectory)
raw_input("Press Enter to continue...")
def move_to_home(self):
wpose = self.move_group.get_current_pose().pose
wpose.position.x = 0.0
wpose.position.y = 0.0
wpose.position.z = 0.0
wpose.orientation.w = 0.0
wpose.orientation.x = 0.0
wpose.orientation.y = 0.0
wpose.orientation.z = 0.0
self.move_group.set_pose_target(wpose)
result = self.move_group.go(wait=True)
def move_to_seam_start(self, motion_point, sine):
pose_goal = Pose()
rpy = [math.pi, 0.0, sine - math.pi / 2]
print(rpy)
R_result = rox.rpy2R(rpy)
quat = rox.R2q(R_result)
print(quat)
pose_goal.orientation.w = 0.0
pose_goal.orientation.x = quat[1]
pose_goal.orientation.y = quat[2]
pose_goal.orientation.z = 0.0
#20- sets setpoint in middle of dxf file for robot y axis
#middle of dxf y axis is 0, so no centering needed here
x_val = (20 - motion_point[0]) * 0.0254
y_val = motion_point[1] * 0.0254
pose_goal.position.x = 0.8 + y_val
pose_goal.position.y = x_val
pose_goal.position.z = 0.3
pose_goal.position.x += 0.1
#self.robot.set_start_state_to_current_state()
self.move_group.set_pose_target(pose_goal)
result = self.move_group.go(wait=True)
def reset_move(self):
wpose = self.move_group.get_current_pose().pose
wpose.position.x = 0.7
wpose.position.y = 0.56263286
wpose.position.z = 0.3
wpose.orientation.w = -0.000164824011947
wpose.orientation.x = 1.0
wpose.orientation.y = 0.0
wpose.orientation.z = 0.0
self.move_group.set_pose_target(wpose)
result = self.move_group.go(wait=True)
def execute_consecutive_motions(self, points):
i = 0
#wpose = self.move_group.get_current_pose().pose
#self.move_group.set_pose_target(wpose)
#plan=self.move_group.go(wpose,wait=True)
#self.move_group.execute(plan)
for i in points:
self.move_group.set_pose_target(i)
raw_input("Press Enter to continue...")
plan = self.move_group.go(wait=True)
self.move_group.stop()
#self.display_robot_trajectory(plan)
#self.move_group.execute(plan)
def execute_planned_trajectory(self, plan):
#wpose = self.move_group.get_current_pose().pose
#self.move_group.set_pose_target(wpose)
#time.sleep(0.5)
#result=self.move_group.go(wait=True)
#self.move_group.stop()
#print("plan:")
#print(plan)
message = rospy.wait_for_message("/robot_status", RobotStatus)
joint_state = rospy.wait_for_message("/joint_states", JointState)
while (message.in_motion.val == 1):
print(message)
message = rospy.wait_for_message("/robot_status", RobotStatus)
#self.move_group.clear_pose_targets()
#print(joint_state)
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
self.move_group.set_start_state(moveit_robot_state)
#add check to see if plan start state agrees with start state or consider adding back in getting current pose and appending it to waypoints potentially
result = self.move_group.execute(plan, wait=True)
#print(result)
if (not result):
print("not executed")
self.execute_seam_response_pub.publish(
"Execution Failed initially but retrying")
wpose = self.move_group.get_current_pose().pose
#print(wpose)
wpose.position.z += 0.1
self.move_group.set_pose_target(wpose)
result = self.move_group.go(wait=True)
plan, fraction, waypoints = self.plan_robot_motion(
self.dxf_points, self.sines)
self.execute_planned_trajectory(plan)
else:
self.execute_seam_response_pub.publish("Executed Successfully")
#time.sleep(1)
def plan_robot_motion(self, dxf_points, sines):
self.dxf_points = dxf_points
self.sines = sines
waypoints = []
message = rospy.wait_for_message("/robot_status", RobotStatus)
joint_state = rospy.wait_for_message("/joint_states", JointState)
while (message.in_motion.val == 1):
message = rospy.wait_for_message("/robot_status", RobotStatus)
#self.move_group.clear_pose_targets()
print("hello")
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
self.move_group.set_start_state(moveit_robot_state)
#waypoints.append(self.move_group.get_current_pose().pose)
#wpose = self.move_group.get_current_pose().pose
#waypoints.append(wpose)
#state = self.robot.get_current_state()
#self.move_group.set_start_state(state)
for i in range(len(dxf_points)):
pose_goal = Pose()
rpy = [math.pi, 0.0, sines[i] - math.pi / 2]
print(rpy)
R_result = rox.rpy2R(rpy)
quat = rox.R2q(R_result)
print(quat)
pose_goal.orientation.w = 0.0
pose_goal.orientation.x = quat[1]
pose_goal.orientation.y = quat[2]
pose_goal.orientation.z = 0.0
#20- sets setpoint in middle of dxf file for robot y axis
#middle of dxf y axis is 0, so no centering needed here
x_val = (20 - dxf_points[i][0]) * 0.0254
y_val = dxf_points[i][1] * 0.0254
pose_goal.position.x = 0.8 + y_val
pose_goal.position.y = x_val
pose_goal.position.z = 0.3
print(pose_goal)
waypoints.append(pose_goal)
"""
euclidean_distances=[]
for i in range(1,len(waypoints)):
distance=pow(pow(waypoints[i].position.x-waypoints[i-1].position.x,2)+pow(waypoints[i].position.y-waypoints[i-1].position.y,2)+pow(waypoints[i].position.z-waypoints[i-1].position.z,2),0.5)
print(distance)
euclidean_distances.append(distance)
error_code=None
"""
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print(type(plan))
#self.scene.motion_plan_request
replan_value = 0
while (replan_value < 3 and fraction < 1.0):
print(fraction)
(plan, fraction) = self.move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
replan_value += 1
print("WARNING Portion of plan failed, replanning")
if (replan_value > 2):
return 0, 0, 0
#print(len(euclidean_distances))
#print(len(plan.joint_trajectory.points))
#print(waypoints[0])
#print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions))
#print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(3))
#print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(7))
#print(self.kdl_kin.forward(plan.joint_trajectory.points[0].positions).item(11))
#print(plan.joint_trajectory.points)
total_distance = 0
distances = []
for i in range(1, len(plan.joint_trajectory.points)):
old_cartesian = self.kdl_kin.forward(
plan.joint_trajectory.points[i - 1].positions)
new_cartesian = self.kdl_kin.forward(
plan.joint_trajectory.points[i].positions)
distance = pow(
pow(new_cartesian.item(3) - old_cartesian.item(3), 2) +
pow(new_cartesian.item(7) - old_cartesian.item(7), 2) +
pow(new_cartesian.item(11) - old_cartesian.item(11), 2), 0.5)
distances.append(distance)
total_distance += distance
#new_time=plan.joint_trajectory.points[i].time_from_start.to_sec()+(distance/self.speed)
#old_time=plan.joint_trajectory.points[i].time_from_start.to_sec()
#print("new time")
#print(new_time)
#print(distance/self.speed)
#print(old_time)
#if(new_time > old_time):
# plan.joint_trajectory.points[i].time_from_start.from_sec(new_time)
#else:
# print("Error, speed faster than possible execution time")
print(total_distance)
print(distances)
total_time = plan.joint_trajectory.points[-1].time_from_start.to_sec()
print(total_time)
times = []
for i in distances:
new_time = (i / total_distance) * total_time
times.append(new_time)
"""
if(new_timestamp > old_timestamp)
next_waypoint->time_from_start.fromSec(new_timestamp);
else
{
//ROS_WARN_NAMED("setAvgCartesianSpeed", "Average speed is too fast. Moving as fast as joint constraints allow.");
}"""
#print point.time_from_start.secs
#for i in range(len(plan.joint_trajectory.points)-1):
# print("time between points:\n")
# print(plan.joint_trajectory.points[i+1].time_from_start.nsecs-plan.joint_trajectory.points[i].time_from_start.nsecs)
#for i in range(1,len(plan.joint_trajectory.points)-1):
# plan.joint_trajectory.points[i].velocities=[-0.1,0.05,0.11,0.00000001,-0.05,0.2]
#plan=self.move_group.retime_trajectory(moveit_robot_state,plan,velocity_scaling_factor=1.0, algorithm="iterative_spline_parameterization")
if (self.display):
self.display_robot_trajectory(plan)
#self.actiongoal= control_msgs.msg.FollowJointTrajectoryActionGoal()
self.goal = control_msgs.msg.FollowJointTrajectoryGoal()
#self.actiongoal.header=std_msgs.msg.Header()
self.goal.trajectory.joint_names = joint_state.name
#self.goal.trajectory.points.resize(len(plan.joint_trajectory.points))
traj = Trajectory()
time = 0.01
traj.add_point(plan.joint_trajectory.points[0].positions, time)
#time=0.0
for i in range(1, len(plan.joint_trajectory.points)):
time += times[i - 1]
traj.add_point(plan.joint_trajectory.points[i].positions, time)
#traj.start()
#traj.wait(15.0)
"""
trajectory_point=trajectory_msgs.msg.JointTrajectoryPoint()
trajectory_point.positions=plan.joint_trajectory.points[i].positions
trajectory_point.velocities=plan.joint_trajectory.points[i].velocities
#trajectory_point.accelerations=plan.joint_trajectory.points[i].accelerations
trajectory_point.time_from_start=plan.joint_trajectory.points[i].time_from_start
self.goal.trajectory.points.append(trajectory_point)
print(len(plan.joint_trajectory.points))
self.actiongoal.goal=self.goal
self.followjointaction.publish(self.actiongoal)
print("published goal")
"""
return plan, fraction, waypoints
def dxf_grabber_readfile(self, data):
filename = data.data
dxf = dxfgrabber.readfile(filename)
layer_count = len(dxf.layers)
self.seam_point_dict = {}
#print(layer_count)
#print(len(dxf.blocks))
for entity in dxf.entities:
print(entity.dxftype)
all_blocks = [
entity for entity in dxf.entities if entity.dxftype == 'INSERT'
]
all_splines = [
entity for entity in dxf.entities if entity.dxftype == 'SPLINE'
]
#print(all_splines)
if (len(all_blocks) != 0):
test_block = dxf.blocks[all_blocks[0].name]
all_polylines = [
entity for entity in test_block if entity.dxftype == 'POLYLINE'
]
else:
all_polylines = [
entity for entity in dxf.entities
if entity.dxftype == 'POLYLINE'
]
#print(len(all_polylines))
x = 0
last_in = False
removed = []
flag = False
vertices = []
sine_vals = []
#with open('eggs.csv', 'w') as csvfile:
# spamwriter = csv.writer(csvfile, delimiter=' ',
# quotechar='|', quoting=csv.QUOTE_MINIMAL)
for i in range(len(all_polylines)):
start = all_polylines[i].points[0]
end = all_polylines[i].points[-1]
for e in range(len(all_polylines)):
if (i == e):
continue
else:
if (all_polylines[e].points[0] == start
and all_polylines[e].points[-1] == end):
#print("identical start found")
#print(start)
#print(i)
#print(e)
#if(all_polylines[e].points[-1]==end):
#print("identical end found")
#print(end)
#print(i)
#print(e)
if (len(all_polylines[i].points) == len(
all_polylines[e].points) and not flag):
flag = True
removed.append(all_polylines[e])
if (len(all_polylines[i].points) > len(
all_polylines[e].points)):
#print(len(all_polylines[i].points))
#print(len(all_polylines[e].points))
removed.append(all_polylines[e])
#print(vertices)
outside = 0
for entry in removed:
all_polylines.remove(entry)
#print(len(all_polylines))
for i in range(len(all_polylines)):
for point in all_polylines[i].points:
vertices.append(point)
try:
shape = matplotlib.path.Path(vertices, closed=True)
except:
self.dxf_file_response_pub.publish(
String(
"dxf file chosen does not contain a closed path, are you sure the file is valid?"
))
colors = ['b', 'g', 'r', 'c', 'm', 'y', 'k']
seam_counter = 0
for entity in all_polylines:
color = colors[x]
sine_vals = []
motion_points = []
for i in range(len(entity.points) - 1):
plt.plot([entity.points[i][0], entity.points[i + 1][0]],
[entity.points[i][1], entity.points[i + 1][1]], color)
#spamwriter.writerow("%f %f"%(entity.points[i][0],entity.points[i][1]))
if (i == 0):
magnitude = self.distance_calculator(
entity.points[i], entity.points[i + 1])
x_vals = [entity.points[i][0], entity.points[i + 1][0]]
y_vals = [entity.points[i][1], entity.points[i + 1][1]]
else:
magnitude = self.distance_calculator(
entity.points[i - 1], entity.points[i + 1])
x_vals = [
entity.points[i - 1][0], entity.points[i][0],
entity.points[i + 1][0]
]
y_vals = [
entity.points[i - 1][1], entity.points[i][1],
entity.points[i + 1][1]
]
#bestfitlineslope, offset=np.polyfit(x_vals,y_vals,1)
#m=(entity.points[i+1][1]-entity.points[i][1])/(entity.points[i+1][0]-entity.points[i][0])
#print(entity.points[i+1][0]-entity.points[i][0])
#print(magnitude/bestfitlineslope)
bestfitlineslope, offset = np.polyfit(x_vals, y_vals, 1)
#print("best fit line:")
#print(bestfitlineslope)
#print(shape.contains_point([entity.points[i][0]+bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),entity.points[i][1]+1/(math.sqrt(bestfitlineslope**2+1))]))
#plt.arrow(entity.points[i][0],entity.points[i][1],bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),1/(math.sqrt(bestfitlineslope**2+1)))
#plt.plot(entity.points[i][0]+bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),entity.points[i][1]+1/(math.sqrt(bestfitlineslope**2+1)),'bo')
if (shape.contains_point([
entity.points[i][0] + bestfitlineslope /
(-math.sqrt(bestfitlineslope**2 + 1)),
entity.points[i][1] + 1 /
(math.sqrt(bestfitlineslope**2 + 1))
])):
outside += 1
value = bestfitlineslope / (
math.sqrt(bestfitlineslope**2 +
1)) / (-1 /
(math.sqrt(bestfitlineslope**2 + 1)))
plt.arrow(
entity.points[i][0], entity.points[i][1],
bestfitlineslope /
(math.sqrt(bestfitlineslope**2 + 1)),
-1 / (math.sqrt(bestfitlineslope**2 + 1)))
sine_vals.append(
math.atan2(
-1 / (math.sqrt(bestfitlineslope**2 + 1)),
bestfitlineslope /
(math.sqrt(bestfitlineslope**2 + 1))))
else:
value = bestfitlineslope / (
-math.sqrt(bestfitlineslope**2 + 1)) / (
1 / (math.sqrt(bestfitlineslope**2 + 1)))
#print([entity.points[i][0]+bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),entity.points[i][1]+1/(math.sqrt(bestfitlineslope**2+1))])
plt.arrow(
entity.points[i][0], entity.points[i][1],
bestfitlineslope /
(-math.sqrt(bestfitlineslope**2 + 1)),
1 / (math.sqrt(bestfitlineslope**2 + 1)))
sine_vals.append(
math.atan2(
1 / (math.sqrt(bestfitlineslope**2 + 1)),
bestfitlineslope /
(-math.sqrt(bestfitlineslope**2 + 1))))
#plt.plot(entity.points[i][0]+bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),entity.points[i][1]+1/(math.sqrt(bestfitlineslope**2+1)),'bo')
#plt.arrow(entity.points[i][0],entity.points[i][1],-(entity.points[i+1][1]-entity.points[i][1])/magnitude,(entity.points[i+1][0]-entity.points[i][0])/magnitude)
#print(distance_calculator(entity.points[i],[entity.points[i][0]+bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),entity.points[i][1]+1/(math.sqrt(bestfitlineslope**2+1))]))
#print(shape.contains_point([entity.points[i][0]+bestfitlineslope/(-math.sqrt(bestfitlineslope**2+1)),entity.points[i][1]+1/(math.sqrt(bestfitlineslope**2+1))]))
#print(end)
#plt.plot([entity.points[i][0],entity.points[i][1]],[entity.points[i][0]-end,entity.points[i][1]+end])
motion_points.append(entity.points[i])
x_vals = [entity.points[-2][0], entity.points[-1][0]]
y_vals = [entity.points[-2][1], entity.points[-1][1]]
bestfitlineslope, offset = np.polyfit(x_vals, y_vals, 1)
if (shape.contains_point([
entity.points[i][0] + bestfitlineslope /
(-math.sqrt(bestfitlineslope**2 + 1)), entity.points[i][1] +
1 / (math.sqrt(bestfitlineslope**2 + 1))
])):
outside += 1
value = bestfitlineslope / (
math.sqrt(bestfitlineslope**2 +
1)) / (-1 / (math.sqrt(bestfitlineslope**2 + 1)))
plt.arrow(
entity.points[-1][0], entity.points[-1][1],
bestfitlineslope / (math.sqrt(bestfitlineslope**2 + 1)),
-1 / (math.sqrt(bestfitlineslope**2 + 1)))
sine_vals.append(
math.atan2(
-1 / (math.sqrt(bestfitlineslope**2 + 1)),
bestfitlineslope /
(math.sqrt(bestfitlineslope**2 + 1))))
else:
plt.arrow(
entity.points[-1][0], entity.points[-1][1],
bestfitlineslope / (-math.sqrt(bestfitlineslope**2 + 1)),
1 / (math.sqrt(bestfitlineslope**2 + 1)))
sine_vals.append(
math.atan2(
1 / (math.sqrt(bestfitlineslope**2 + 1)),
bestfitlineslope /
(-math.sqrt(bestfitlineslope**2 + 1))))
x += 1
#sine_vals.append(math.asin(value))
motion_points.append(entity.points[-1])
#print(sine_vals)
if (seam_counter == 0):
self.seam_point_dict["bottom_seam"] = [
motion_points, sine_vals
]
elif (seam_counter == 1):
self.seam_point_dict["in_seam"] = [motion_points, sine_vals]
elif (seam_counter == 2):
self.seam_point_dict["j_seam"] = [motion_points, sine_vals]
elif (seam_counter == 3):
self.seam_point_dict["top_seam"] = [motion_points, sine_vals]
elif (seam_counter == 4):
self.seam_point_dict["out_seam"] = [motion_points, sine_vals]
else:
self.dxf_file_response_pub.publish(
String(
"dxf file contained too many seams, check to see if dxf file is valid"
))
#plan, fraction, points=self.plan_robot_motion(motion_points,sine_vals)
#if(fraction<1.0):
# self.execute_consecutive_motions(points)
#else:
#self.display_robot_trajectory(plan)
#self.execute_planned_trajectory(plan)
if (x + 1 > len(colors)):
x = 0
seam_counter += 1
#spamwriter.writerow("")
#print("end")
#print(self.seam_point_dict)
self.dxf_file_response_pub.publish(
String("Successfully parsed dxf file"))
#print(outside)
#plt.show()
"""if(all_lines[0].start[0]>all_lines[1].start[0]):
class ManipulatorSimpleModel:
def __init__(self,
urdf_file_name,
base_link="base_link",
ee_name=_EE_NAME):
'''
Simple model of manipulator kinematics and controls
Assume following state and action vectors
urdf_file_name - model file to load
'''
# Load KDL tree
urdf_file = file(urdf_file_name, 'r')
self.robot = Robot.from_xml_string(urdf_file.read())
urdf_file.close()
self.tree = kdl_tree_from_urdf_model(self.robot)
task_space_ik_weights = np.diag([1.0, 1.0, 1.0, 0.0, 0.0,
0.0]).tolist()
#self.base_link = self.robot.get_root()
self.base_link = base_link
self.joint_chains = []
self.chain = KDLKinematics(self.robot, self.base_link, ee_name)
'''
for l_name in _LINK_NAMES:
jc=KDLKinematics(self.robot,self.base_link,l_name)
self.joint_chains.append(jc)
'''
def FK_joint(self, joint_angles, j_index):
'''
Method to return task coordinates between base link and any joint
joint_angles must contain only 0:j_index joints
'''
fi_x = self.joint_chains[j_index].forward(joint_angles)
return fi_x
def Jacobian_joint(self, joint_angles, j_index):
ji_x = self.joint_chains[j_index].jacobian(joint_angles)
return ji_x
def FK(self, joint_angles):
'''
Method to convert joint positions to task coordinates
'''
fi_x = self.chain.forward(joint_angles)
return fi_x
def Jacobian(self, joint_angles):
ji_x = self.chain.jacobian(joint_angles)
return ji_x
#TODO: CHECK if IK works
def IK(self, fingers_desired, finger=None):
'''
Get inverse kinematics for desired finger poses of all fingers
fingers_desired - a list of desired finger tip poses in order of finger_chains[]
returns - a list of lists of pyhton arrays of finger joint configurations
TODO: Allow for single finger computation (named fingers)
'''
q_desired_fingers = []
if finger is not None:
# TODO: solve for a single finger...
return None
for i, f_d in enumerate(fingers_desired):
q_desired_fingers.append(self.finger_chains[i].inverse_wdls(f_d))
# q_desired_fingers.append(self.finger_chains[i].inverse(f_d))
return q_desired_fingers
class SimplePlanning:
skip_tol = 1e-6
# How you set these options will determine how we do planning:
# what inverse kinematics are used for queries, etc. Most of these are
# meant to be directly inherited/provided by the CoSTAR Arm class.
def __init__(self,
robot,
base_link,
end_link,
group,
move_group_ns="move_group",
planning_scene_topic="planning_scene",
robot_ns="",
verbose=False,
kdl_kin=None,
closed_form_IK_solver=None,
joint_names=[]):
self.robot = robot
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(base_link, end_link)
if kdl_kin is None:
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
else:
self.kdl_kin = kdl_kin
self.base_link = base_link
self.joint_names = joint_names
self.end_link = end_link
self.group = group
self.robot_ns = robot_ns
self.client = actionlib.SimpleActionClient(move_group_ns,
MoveGroupAction)
self.acceleration_magnification = 1
rospy.wait_for_service('compute_cartesian_path')
self.cartesian_path_plan = rospy.ServiceProxy('compute_cartesian_path',
GetCartesianPath)
self.verbose = verbose
self.closed_form_IK_solver = closed_form_IK_solver
# Basic ik() function call.
# It handles calls to KDL inverse kinematics or to the closed form ik
# solver that you provided.
def ik(self, T, q0, dist=0.5):
q = None
if self.closed_form_IK_solver is not None:
#T = pm.toMatrix(F)
q = self.closed_form_IK_solver.findClosestIK(T, q0)
else:
q = self.kdl_kin.inverse(T, q0)
return q
# Compute parameters for a nice trapezoidal motion. This will let us create
# movements with the basic move() operation that actually look pretty nice.
def calculateAccelerationProfileParameters(
self,
dq_to_target, # joint space offset to target
base_steps, # number of trajectory points to create
steps_per_meter, # number of extra traj pts to make
steps_per_radians, # as above
delta_translation,
time_multiplier,
percent_acc):
# We compute a number of steps to take along our trapezoidal trajectory
# curve. This gives us a nice, relatively dense trajectory that we can
# introspect on later -- we can use it to compute cost functions, to
# detect collisions, etc.
delta_q_norm = np.linalg.norm(dq_to_target)
steps = base_steps + delta_translation * steps_per_meter + delta_q_norm \
* steps_per_radians
# Number of steps must be an int.
steps = int(np.round(steps))
# This is the time needed for constant velocity at 100% to reach the goal.
t_v_constant = delta_translation + delta_q_norm
ts = (t_v_constant / steps) * time_multiplier
# the max constant joint velocity
dq_max_target = np.max(np.absolute(dq_to_target))
v_max = dq_max_target / t_v_constant
v_setting_max = v_max / time_multiplier
acceleration = v_max * percent_acc
# j_acceleration = np.array(dq_target) / t_v_constant * percent_acc
t_v_setting_max = v_setting_max / acceleration
# Compute the number of trajectory points we want to make before we will
# get up to max speed.
steps_to_max_speed = 0.5 * acceleration * t_v_setting_max **2 \
/ (dq_max_target / steps)
if steps_to_max_speed * 2 > steps:
rospy.logwarn("Cannot reach the maximum velocity setting, steps "
"required %.1f > total number of steps %d" %
(steps_to_max_speed * 2, steps))
t_v_setting_max = np.sqrt(0.5 * dq_max_target / acceleration)
steps_to_max_speed = (0.5 * steps)
v_setting_max = t_v_setting_max * acceleration
rospy.loginfo("Acceleration number of steps is set to %.1f and time "
"elapsed to reach max velocity is %.3fs" %
(steps_to_max_speed, t_v_setting_max))
const_velocity_max_step = np.max([steps - 2 * steps_to_max_speed, 0])
return steps, ts, t_v_setting_max, steps_to_max_speed, const_velocity_max_step
# This is where we compute what time we want for each trajectory point.
def calculateTimeOfTrajectoryStep(self, step_index, steps_to_max_speed,
const_velocity_max_step, t_v_const_step,
t_to_reach_v_setting_max):
acceleration_step = np.min([step_index, steps_to_max_speed])
deceleration_step = np.min([
np.max(
[step_index - const_velocity_max_step - steps_to_max_speed,
0]), steps_to_max_speed
])
const_velocity_step = np.min([
np.max([0, step_index - steps_to_max_speed]),
const_velocity_max_step
])
acceleration_time = 0
deceleration_time = 0
if steps_to_max_speed > 0.0001:
acceleration_time = np.sqrt(
acceleration_step /
steps_to_max_speed) * t_to_reach_v_setting_max
deceleration_time = t_to_reach_v_setting_max - np.sqrt(
(steps_to_max_speed - deceleration_step) /
steps_to_max_speed) * t_to_reach_v_setting_max
const_vel_time = const_velocity_step * t_v_const_step
return acceleration_time + const_vel_time + deceleration_time
# Compute a nice joint trajectory. This is useful for checking collisions,
# and ensuring that we have nice, well defined behavior.
def getJointMove(self,
q_goal,
q0,
base_steps=1000,
steps_per_meter=1000,
steps_per_radians=4,
time_multiplier=1,
percent_acc=1,
use_joint_move=False,
table_frame=None):
if q0 is None:
rospy.logerr("Invalid initial joint position in getJointMove")
return JointTrajectory()
elif np.all(np.isclose(q0, q_goal, atol=0.0001)):
rospy.logwarn("Robot is already in the goal position.")
return JointTrajectory()
if np.any(np.greater(np.absolute(q_goal[:2] - np.array(q0[:2])), np.pi/2)) \
or np.absolute(q_goal[3] - q0[3]) > np.pi:
# TODO: these thresholds should not be set manually here.
rospy.logerr("Dangerous IK solution, abort getJointMove")
return JointTrajectory()
delta_q = np.array(q_goal) - np.array(q0)
# steps = base_steps + int(np.sum(np.absolute(delta_q)) * steps_per_radians)
steps, t_v_const_step, t_v_setting_max, steps_to_max_speed, const_velocity_max_step = self.calculateAccelerationProfileParameters(
delta_q, base_steps, 0, steps_per_radians, 0, time_multiplier,
self.acceleration_magnification * percent_acc)
traj = JointTrajectory()
traj.points.append(
JointTrajectoryPoint(positions=q0,
velocities=[0] * len(q0),
accelerations=[0] * len(q0)))
# compute IK
for i in range(1, steps + 1):
xyz = None
rpy = None
q = None
q = np.array(q0) + (float(i) / steps) * delta_q
q = q.tolist()
if self.verbose:
print "%d -- %s %s = %s" % (i, str(xyz), str(rpy), str(q))
if q is not None:
dq_i = np.array(q) - np.array(traj.points[i - 1].positions)
if np.sum(dq_i) < 0.0001:
rospy.logwarn(
"Joint trajectory point %d is repeating previous trajectory point. "
% i)
# continue
total_time = total_time = self.calculateTimeOfTrajectoryStep(
i, steps_to_max_speed, const_velocity_max_step,
t_v_const_step, t_v_setting_max)
traj.points[i - 1].velocities = dq_i / (
total_time - traj.points[i - 1].time_from_start.to_sec())
pt = JointTrajectoryPoint(positions=q,
velocities=[0] * len(q),
accelerations=[0] * len(q))
pt.time_from_start = rospy.Duration(total_time)
traj.points.append(pt)
else:
rospy.logwarn(
"No IK solution on one of the trajectory point to cartesian move target"
)
if len(traj.points) < base_steps:
print rospy.logerr("Planning failure with " \
+ str(len(traj.points)) \
+ " / " + str(base_steps) \
+ " points.")
return JointTrajectory()
traj.joint_names = self.joint_names
return traj
# Compute a simple trajectory.
def getCartesianMove(self,
frame,
q0,
base_steps=1000,
steps_per_meter=1000,
steps_per_radians=4,
time_multiplier=1,
percent_acc=1,
use_joint_move=False,
table_frame=None):
if table_frame is not None:
if frame.p[2] < table_frame[0][2]:
rospy.logerr(
"Ignoring move to waypoint due to relative z: %f < %f" %
(frame.p[2], table_frame[0][2]))
return JointTrajectory()
if q0 is None:
rospy.logerr("Invalid initial joint position in getCartesianMove")
return JointTrajectory()
# interpolate between start and goal
pose = pm.fromMatrix(self.kdl_kin.forward(q0))
cur_rpy = np.array(pose.M.GetRPY())
cur_xyz = np.array(pose.p)
goal_rpy = np.array(frame.M.GetRPY())
goal_xyz = np.array(frame.p)
delta_rpy = np.linalg.norm(goal_rpy - cur_rpy)
delta_translation = (pose.p - frame.p).Norm()
if delta_rpy < 0.001 and delta_translation < 0.001:
rospy.logwarn("Robot is already in the goal position.")
return JointTrajectory(points=[
JointTrajectoryPoint(positions=q0,
velocities=[0] * len(q0),
accelerations=[0] * len(q0),
time_from_start=rospy.Duration(0.0))
],
joint_names=self.joint_names)
q_target = self.ik(pm.toMatrix(frame), q0)
if q_target is None:
rospy.logerr("No IK solution on cartesian move target")
return JointTrajectory()
else:
if np.any(
np.greater(
np.absolute(q_target[:2] - np.array(q0[:2])), np.pi/2)) \
or np.absolute(q_target[3] - q0[3]) > np.pi:
rospy.logerr("Dangerous IK solution, abort getCartesianMove")
return JointTrajectory()
dq_target = q_target - np.array(q0)
if np.sum(np.absolute(dq_target)) < 0.0001:
rospy.logwarn("Robot is already in the goal position.")
return JointTrajectory(points=[
JointTrajectoryPoint(positions=q0,
velocities=[0] * len(q0),
accelerations=[0] * len(q0),
time_from_start=rospy.Duration(0.0))
],
joint_names=self.joint_names)
steps, t_v_const_step, t_v_setting_max, steps_to_max_speed, const_velocity_max_step = self.calculateAccelerationProfileParameters(
dq_target, base_steps, steps_per_meter, steps_per_radians,
delta_translation, time_multiplier,
self.acceleration_magnification * percent_acc)
traj = JointTrajectory()
traj.points.append(
JointTrajectoryPoint(positions=q0,
velocities=[0] * len(q0),
accelerations=[0] * len(q0)))
# Compute a smooth trajectory.
for i in range(1, steps + 1):
xyz = None
rpy = None
q = None
if not use_joint_move:
xyz = cur_xyz + ((float(i) / steps) * (goal_xyz - cur_xyz))
rpy = cur_rpy + ((float(i) / steps) * (goal_rpy - cur_rpy))
# Create transform for goal frame
frame = pm.toMatrix(
kdl.Frame(kdl.Rotation.RPY(rpy[0], rpy[1], rpy[2]),
kdl.Vector(xyz[0], xyz[1], xyz[2])))
# Use current inverse kinematics solver with current position
q = self.ik(frame, q0)
else:
q = np.array(q0) + (float(i) / steps) * dq_target
q = q.tolist()
#q = self.kdl_kin.inverse(frame,q0)
if self.verbose:
print "%d -- %s %s = %s" % (i, str(xyz), str(rpy), str(q))
if q is not None:
total_time = self.calculateTimeOfTrajectoryStep(
i, steps_to_max_speed, const_velocity_max_step,
t_v_const_step, t_v_setting_max)
# Compute the distance to the last point for each joint. We use this to compute our joint velocities.
dq_i = np.array(q) - np.array(traj.points[-1].positions)
if np.sum(np.abs(dq_i)) < self.skip_tol:
rospy.logwarn(
"Joint trajectory point %d is repeating previous trajectory point. "
% i)
continue
traj.points[i - 1].velocities = (dq_i) / (
total_time - traj.points[i - 1].time_from_start.to_sec())
pt = JointTrajectoryPoint(positions=q,
velocities=[0] * len(q),
accelerations=[0] * len(q))
pt.time_from_start = rospy.Duration(total_time)
# pt.time_from_start = rospy.Duration(i * ts)
traj.points.append(pt)
else:
rospy.logwarn(
"No IK solution on one of the trajectory point to cartesian move target"
)
if len(traj.points) < base_steps:
print rospy.logerr("Planning failure with " \
+ str(len(traj.points)) \
+ " / " + str(base_steps) \
+ " points.")
return JointTrajectory()
traj.joint_names = self.joint_names
return traj
def getGoalConstraints(self,
frame=None,
q=None,
q_goal=None,
timeout=2.0,
mode=ModeJoints):
if frame == None and q_goal == None:
raise RuntimeError(
'Must provide either a goal frame or joint state!')
return (None, None)
if q is None:
raise RuntimeError('Must provide starting position!')
return (None, None)
if len(self.robot_ns) > 0:
srv = rospy.ServiceProxy(self.robot_ns + "/compute_ik",
moveit_msgs.srv.GetPositionIK)
else:
srv = rospy.ServiceProxy("compute_ik",
moveit_msgs.srv.GetPositionIK)
goal = Constraints()
if frame is not None:
joints = self.ik(pm.toMatrix(frame), q)
elif q_goal is not None:
joints = q_goal
if len(joints) is not len(self.joint_names):
rospy.logerr(
"Invalid goal position. Number of joints in goal is not the same as robot's dof"
)
return (None, None)
if mode == ModeJoints or q_goal is not None:
for i in range(0, len(self.joint_names)):
joint = JointConstraint()
joint.joint_name = self.joint_names[i]
joint.position = joints[i]
joint.tolerance_below = 1e-6
joint.tolerance_above = 1e-6
joint.weight = 1.0
goal.joint_constraints.append(joint)
else:
print 'Setting cartesian constraint'
# TODO: Try to fix this again. Something is wrong
cartesian_costraint = PositionConstraint()
cartesian_costraint.header.frame_id = 'base_link'
cartesian_costraint.link_name = self.joint_names[-1]
# cartesian_costraint.target_point_offset = frame.p
bounding_volume = BoundingVolume()
sphere_bounding = SolidPrimitive()
sphere_bounding.type = sphere_bounding.SPHERE
# constrain position with sphere 1 mm around target
sphere_bounding.dimensions.append(0.5)
bounding_volume.primitives.append(sphere_bounding)
sphere_pose = Pose()
sphere_pose.position = frame.p
sphere_pose.orientation.w = 1.0
bounding_volume.primitive_poses.append(sphere_pose)
cartesian_costraint.constraint_region = bounding_volume
cartesian_costraint.weight = 1.0
goal.position_constraints.append(cartesian_costraint)
orientation_costraint = OrientationConstraint()
orientation_costraint.header.frame_id = 'base_link'
orientation_costraint.link_name = self.joint_names[-1]
orientation_costraint.orientation = frame.M.GetQuaternion()
orientation_costraint.absolute_x_axis_tolerance = 0.1
orientation_costraint.absolute_y_axis_tolerance = 0.1
orientation_costraint.absolute_z_axis_tolerance = 0.1
orientation_costraint.weight = 1.0
goal.orientation_constraints.append(orientation_costraint)
print 'Done'
return (None, goal)
def updateAllowedCollisions(self, obj, allowed):
self.planning_scene_publisher = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
rospy.wait_for_service('get_planning_scene', 10.0)
get_planning_scene = rospy.ServiceProxy('get_planning_scene',
GetPlanningScene)
request = PlanningSceneComponents(
components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
if not obj in acm.default_entry_names:
# add button to allowed collision matrix
acm.default_entry_names += [obj]
acm.default_entry_values += [allowed]
else:
idx = acm.default_entry_names.index(obj)
acm.default_entry_values[idx] = allowed
#if obj in acm.entry_names:
# idx = acm.entry_names.idx
# for entry in acm.entry_values:
# entry.enabled[idx] = allowed
# for i in xrange(len(acm.entry_names)):
# acm.entry_values[idx].enabled[i]=allowed
planning_scene_diff = PlanningScene(is_diff=True,
allowed_collision_matrix=acm)
self.planning_scene_publisher.publish(planning_scene_diff)
def getPlan(self,
frame=None,
q=None,
q_goal=None,
obj=None,
compute_ik=True):
planning_options = PlanningOptions()
planning_options.plan_only = True
planning_options.replan = False
planning_options.replan_attempts = 0
planning_options.replan_delay = 0.1
planning_options.planning_scene_diff.is_diff = True
planning_options.planning_scene_diff.robot_state.is_diff = True
if frame is None and q_goal is None:
raise RuntimeError(
'Must provide either a goal frame or joint state!')
if q is None:
raise RuntimeError('Must provide starting position!')
elif len(q) is not len(self.joint_names):
rospy.logerr(
"Invalid number of joints in getPlan starting position setting"
)
return (-31, None)
if obj is not None:
self.updateAllowedCollisions(obj, True)
motion_req = MotionPlanRequest()
motion_req.start_state.joint_state.position = q
motion_req.start_state.joint_state.name = self.joint_names
motion_req.workspace_parameters.header.frame_id = self.base_link
motion_req.workspace_parameters.max_corner.x = 1.0
motion_req.workspace_parameters.max_corner.y = 1.0
motion_req.workspace_parameters.max_corner.z = 1.0
motion_req.workspace_parameters.min_corner.x = -1.0
motion_req.workspace_parameters.min_corner.y = -1.0
motion_req.workspace_parameters.min_corner.z = -1.0
# create the goal constraints
# TODO: change this to use cart goal(s)
# - frame: take a list of frames
# - returns: goal contraints
constrain_mode = ModeJoints
if compute_ik:
(ik_resp, goal) = self.getGoalConstraints(frame=frame,
q=q,
q_goal=q_goal,
mode=constrain_mode)
motion_req.goal_constraints.append(goal)
motion_req.group_name = self.group
motion_req.num_planning_attempts = 10
motion_req.allowed_planning_time = 4.0
motion_req.planner_id = "RRTConnectkConfigDefault"
if goal is None:
print 'Error: goal is None'
return (-31, None)
elif constrain_mode == ModeJoints and motion_req is not None and len(
motion_req.goal_constraints[0].joint_constraints) == 0:
print 'Error: joint constraints length is 0'
return (-31, None)
elif ((not ik_resp is None and ik_resp.error_code.val < 0)
or (not ik_resp is None and ik_resp.error_code.val < 0)):
print 'Error: ik resp failure'
return (-31, None)
goal = MoveGroupGoal()
goal.planning_options = planning_options
goal.request = motion_req
rospy.logwarn("Sending request...")
self.client.send_goal(goal)
self.client.wait_for_result()
res = self.client.get_result()
if res is not None:
rospy.logwarn("Done: " + str(res.error_code.val))
if obj is not None:
self.updateAllowedCollisions(obj, False)
return (res.error_code.val, res)
else:
rospy.logerr("Planning response is None")
return (-31, None)
def getPlanWaypoints(self, waypoints_in_kdl_frame, q, obj=None):
cartesian_path_req = GetCartesianPathRequest()
cartesian_path_req.header.frame_id = self.base_link
cartesian_path_req.start_state = RobotState()
cartesian_path_req.start_state.joint_state.name = self.joint_names
if type(q) is list:
cartesian_path_req.start_state.joint_state.position = q
else:
cartesian_path_req.start_state.joint_state.position = q.tolist()
cartesian_path_req.group_name = self.group
cartesian_path_req.link_name = self.joint_names[-1]
cartesian_path_req.avoid_collisions = False
cartesian_path_req.max_step = 50
cartesian_path_req.jump_threshold = 0
# cartesian_path_req.path_constraints = Constraints()
if obj is not None:
self.updateAllowedCollisions(obj, True)
cartesian_path_req.waypoints = list()
for T in waypoints_in_kdl_frame:
cartesian_path_req.waypoints.append(pm.toMsg(T))
res = self.cartesian_path_plan.call(cartesian_path_req)
if obj is not None:
self.updateAllowedCollisions(obj, False)
return (res.error_code.val, res)
def build_tree(iter,treeA, treeB ,edgesA, edgesB, plot, kdl_kin):
i = 0
while i < iter:
# print(i)
jointsA = np.random.rand(1,7)[0]*[2.5, 2.5, 4, 1.5, 4, 2.5, 4] - [1.25, 1.25, 2.0, .75, 2.0, .75, 2.0]
velA = np.random.rand(1,7)[0]*[3.0,3.0,3.0,3.0,6.0,3.0,6.0] - [1.5,1.5,1.5,1.5,2,1.5,2]
node = nearest_neighbor(jointsA, velA, treeA, i)
treeA = insert_vertex(node,treeA,jointsA,velA,i,1)
edgesA = insert_edge(edgesA, node, i)
jointsB = np.random.rand(1,7)[0]*[2.5, 2.5, 4, 1.5, 4, 2.5, 4] - [1.25, 1.25, 2.0, .75, 2.0, .75, 2.0]
velB = np.random.rand(1,7)[0]*[3.0,3.0,3.0,3.0,6.0,3.0,6.0] - [1.5,1.5,1.5,1.5,2,1.5,2]
node = nearest_neighbor(jointsB, velB, treeB, i)
treeB = insert_vertex(node,treeB,jointsB,velB,i,1)
edgesB = insert_edge(edgesB, node, i)
result = connect_trees(treeA, treeB, i, kdl_kin)
# print(result[0],i)
if (result[0]):
print "iterated up to: "
print i
break
i = i + 1
iterations = i
if (plot):
# plt.close('all')
# fig = plt.figure()
# ax = fig.gca(projection='3d')
# ax.scatter(treeA[0,0],treeA[0,1],treeA[0,2],'r')
# ax.plot(treeA[0:i+2,0],treeA[0:i+2,1],treeA[0:i+2,2],'r.')
# ax.scatter(treeB[0,0],treeB[0,1],treeB[0,2],'b')
# ax.plot(treeB[0:i+2,0],treeB[0:i+2,1],treeB[0:i+2,2],'b.')
# plt.show(block=False)
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
pos3A = np.empty((iterations,3))
for i in range(iterations):
pos3A[i,:]=kdl_kin.forward(treeA[i,:7])[0:3,3].transpose()
pos3B = np.empty((iterations,3))
for i in range(iterations):
pos3B[i,:]=kdl_kin.forward(treeB[i,:7])[0:3,3].transpose()
fig = plt.figure(1)
plt.hold(False)
# ax = fig.gca(projection='3d')
plt.plot(pos3A[:,1],pos3A[:,2],'r.')
plt.hold(True)
plt.plot(pos3A[0,1],pos3A[0,2],'r.',markersize=15)
plt.plot(pos3B[:,1],pos3B[:,2],'b.')
plt.plot(pos3B[0,1],pos3B[0,2],'b.',markersize=15)
plt.xlabel('EE y-coordinate')
plt.ylabel('EE z-coordinate')
plt.title('RRT in end-effector space')
# plt.show(block=False)
if (result[0]):
indA = result[1];
indB = result[2];
# if (plot):
# ax.scatter([treeA[indA,0], treeB[indB,0]],[treeA[indA,1], treeB[indB,1]],[treeA[indA,2], treeB[indB,2]],'g')
else:
indA = 0
indB = 0
if (plot):
# plt.xlim([-1.5, 1.5])
# plt.ylim([-1.5, 1.5])
for k in range(iterations+1):
edge1 = kdl_kin.forward(treeA[edgesA[k,0],:7])[0:3,3].transpose()
edge2 = kdl_kin.forward(treeA[edgesA[k,1],:7])[0:3,3].transpose()
plt.plot([edge1[0,1],edge2[0,1]],[edge1[0,2],edge2[0,2]],'r',linewidth=3)
edge1 = kdl_kin.forward(treeB[edgesB[k,0],:7])[0:3,3].transpose()
edge2 = kdl_kin.forward(treeB[edgesB[k,1],:7])[0:3,3].transpose()
plt.plot([edge1[0,1],edge2[0,1]],[edge1[0,2],edge2[0,2]],'b',linewidth=3)
# plt.show(block=False)
# raw_input('Enter...')
return treeA[0:iterations+2,:], treeB[0:iterations+2,:], edgesA[0:iterations+1,:], edgesB[0:iterations+1,:], indA, indB
class KinematicsTest(object):
"""Test kinematics using KDL"""
def __init__(self):
"""Read robot describtion"""
self.robot_ = URDF.from_parameter_server()
self.kdl_kin_ = KDLKinematics(self.robot_, 'base_link', 'end_effector')
self.cur_pose_ = None
self.cur_jnt_ = None
# Creates the SimpleActionClient, passing the type of the action
self.client_ = actionlib.SimpleActionClient(
'/mitsubishi_arm/mitsubishi_trajectory_controller/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
self.client_.wait_for_server()
self.goal_ = control_msgs.msg.FollowJointTrajectoryGoal()
#time_from_start=rospy.Duration(10)
self.goal_.trajectory.joint_names = [
'j1', 'j2', 'j3', 'j4', 'j5', 'j6'
]
#To be reached 1 second after starting along the trajectory
trajectory_point = trajectory_msgs.msg.JointTrajectoryPoint()
trajectory_point.positions = [0] * 6
trajectory_point.time_from_start = rospy.Duration(0.1)
self.goal_.trajectory.points.append(trajectory_point)
rospy.Subscriber('joint_states', sensor_msgs.msg.JointState,
self.fwd_kinematics_cb)
rospy.Subscriber('command', geometry_msgs.msg.Pose,
self.inv_kinematics_cb)
def fwd_kinematics_cb(self, data):
"""Compute forward kinematics and print it out
:data: TODO
:returns: TODO
"""
self.cur_jnt_ = data.position
self.cur_pose_ = self.kdl_kin_.forward(data.position)
q = np.array(data.position)
new_pose = self.cur_pose_.copy()
new_pose[0, 3] += 0.01
#print "cp:", np.ravel(self.cur_pose_[:3,3])
#print "np:", np.ravel(new_pose[:3,3])
#q_ik = self.kdl_kin_.inverse(new_pose, q)
#print "Q:", q
#print "Q_ik:", q_ik
def inv_kinematics_cb(self, data):
"""calculate the ik given a pose (only xyz right now)
:data: TODO
:returns: TODO
"""
# preserve the rotation
new_pose = self.cur_pose_.copy()
new_pose[0, 3] = data.position.x
new_pose[1, 3] = data.position.y
new_pose[2, 3] = data.position.z
q_ik = self.kdl_kin_.inverse(new_pose, self.cur_jnt_)
print "cur:", np.ravel(self.cur_pose_[:3, 3])
print "req:", np.ravel(new_pose[:3, 3])
print "Q:", self.cur_jnt_
print "Q_ik:", q_ik
if q_ik is not None:
print "Sending goal"
self.goal_.trajectory.points[0].positions = q_ik
self.client_.send_goal(self.goal_)
def __init__(self, name):
self.name = name
self.timer = 0
# the names of the base_link and end_link according to the urdf model
self.base_link = "iiwa_0_link"
self.end_link = "iiwa_adapter"
# initialize a node
rospy.init_node("mission_node")
self.robot = URDF.from_xml_file(
"/home/davoud/kuka_ws/src/kuka/kuka/iiwa/iiwa_description/urdf/iiwa.urdf"
)
self.tree = kdl_tree_from_urdf_model(self.robot)
self.kdl_kin = KDLKinematics(self.robot, self.base_link, self.end_link)
# you can use one of these methods:
# 1: Jacobian Transpose Method (JTM)
# 2: Jacobian Pseudoinverse Method (JPM)
# 3: Damped Least Squares Method (DLSM)
self.used_method = "DLSM"
self.landa = 0.1
# tf broadcaster
self.br = tf.TransformBroadcaster()
# current position of the end effector
self.end_eff_current_pose = np.empty((1, 7))
# transformation matrix between end_effector and camera
self.end_cam_T = np.empty((4, 4))
self.set_end_cam_T()
# transformation matrix between end_effector and gripper
self.end_grip_T = np.empty((4, 4))
self.set_end_grip_T()
# transformation matrix between gripper and target pose
self.grip_target_T = np.empty((4, 4))
self.set_grip_target_T()
# transformation matrix between base and end effector
self.base_end_T = np.empty((4, 4))
# transformation matrix between base and target
self.base_target_T = np.empty((4, 4))
# needed variables for actionlib
self.joint_names = [
"joint1", "joint2", "joint3", "joint4", "joint5", "joint6",
"joint7"
]
# actionlib commands
self.manipulator_client = actionlib.SimpleActionClient(
"/iiwa/follow_joint_trajectory", FollowJointTrajectoryAction)
self.manipulator_client.wait_for_server()
self.trajectory = JointTrajectory()
self.trajectory.joint_names = self.joint_names
self.trajectory.points.append(JointTrajectoryPoint())
# path of end_effector
self.ee_path = MarkerArray()
# path of object
self.object_path = MarkerArray()
# initializ a publisher to publish the marker of detected objet to rviz
self.marker_object_in_base_pub = rospy.Publisher(
"marker_object_in_base", Marker, queue_size=1)
self.marker_object_in_cam_pub = rospy.Publisher("marker_object_in_cam",
Marker,
queue_size=1)
self.marker_eff_path_pub = rospy.Publisher("marker_eff_path",
MarkerArray,
queue_size=1)
self.marker_obj_path_pub = rospy.Publisher("marker_obj_path",
MarkerArray,
queue_size=1)
# initialize the service client for the gripper
rospy.wait_for_service("/sdh_action")
self.joint_config_client = rospy.ServiceProxy("/sdh_action", SDHAction)
# subscribe to "/joint_states" topic which is published by kuka.
rospy.Subscriber("/joint_states", JointState, self.kuka_inf_cb)
# subscribe to "/sphere_coefs" topic which is published by ball detector using color and distance filters
rospy.Subscriber("/sphere_coefs",
Float32MultiArray,
self.marker_inf_cb,
queue_size=1)
rospy.spin()
msg = JointState(name=CONFIG['joints'], position=Q)
pub.publish(msg)
rospy.sleep(0.2)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
pass
if __name__ == '__main__':
rospy.init_node('tom_simple_goto')
pub = rospy.Publisher('joint_states_cmd', JointState, queue_size=1000)
robot = URDF.from_parameter_server()
tree = kdl_tree_from_urdf_model(robot)
chain = tree.getChain(base_link, end_link)
kdl_kin = KDLKinematics(robot, base_link, end_link)
"""
position:
x: 0.648891402264
y: -0.563835865845
z: -0.263676911067
orientation:
x: -0.399888401484
y: 0.916082302699
z: -0.0071291983402
w: -0.0288384391252
"""
trans, rot = (0.64, -0.56, -0.26), (-0.4, 0.92, -0.01, -0.03)
class YoubotArm:
# Constructor.
def __init__(self):
self.moving_to_new_x_pos = False
self.moving_to_new_y_pos = False
self.stop_base_movement = False
self.max_virtual_x_vel = 0.05
self.max_virtual_y_vel = 0.05
self.commanded_virtual_x_pos = 0.0
self.commanded_virtual_y_pos = 0.0
self.commanded_virtual_x_vel = 0.0
self.commanded_virtual_y_vel = 0.0
self.virtual_x_cmd_time_sec = 0.0
self.virtual_y_cmd_time_sec = 0.0
self.virtual_x_running_time_sec = 0.0
self.virtual_y_running_time_sec = 0.0
youbot_urdf = URDF.from_parameter_server()
self.kin_with_virtual = KDLKinematics(youbot_urdf, "virtual", "gripper_palm_link")
self.kin_grasp = KDLKinematics(youbot_urdf, "base_link", "gripper_palm_link")
# Create a timer that will be used to monitor the velocity of the virtual
# joints when we need them to be positioning themselves.
self.vel_monitor_timer = rospy.Timer(rospy.Duration(0.1), self.vel_monitor_timer_callback)
self.arm_joint_pub = rospy.Publisher("arm_1/arm_controller/position_command",JointPositions,queue_size=10)
self.gripper_pub = rospy.Publisher("arm_1/gripper_controller/position_command", JointPositions, queue_size=10)
self.vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
# Give the publishers time to get setup before trying to do any actual work.
rospy.sleep(2)
# Initialize at the candle position.
self.publish_arm_joint_positions(armJointPosCandle)
rospy.sleep(2.0)
# Go through the routine of picking up the block.
self.grasp_routine()
# A callback function to run on a timer that runs the velocity commands for a specified
# amount of time.
#
# Params:
# time_data - the rospy.TimerEvent object passed from the rospy.Timer
def vel_monitor_timer_callback(self, time_data):
delta_t = 0.0
if time_data.last_real != None:
delta_t = time_data.current_real.now().to_sec() - time_data.last_real.now().to_sec()
if self.moving_to_new_x_pos or self.moving_to_new_y_pos:
# --- Start publishing velocity commands --- #
cmd_twist = Twist()
if self.moving_to_new_x_pos:
if self.commanded_virtual_x_pos > 0:
cmd_twist.linear.x = self.max_virtual_x_vel
elif self.commanded_virtual_x_pos < 0:
cmd_twist.linear.x = -self.max_virtual_x_vel
self.virtual_x_running_time_sec += delta_t
self.moving_to_new_x_pos = self.virtual_x_running_time_sec <= self.virtual_x_cmd_time_sec
if self.moving_to_new_y_pos:
if self.commanded_virtual_y_pos > 0:
cmd_twist.linear.y = self.max_virtual_y_vel
elif self.commanded_virtual_y_pos < 0:
cmd_twist.linear.y = -self.max_virtual_y_vel
self.virtual_y_running_time_sec += delta_t
self.moving_to_new_y_pos = self.virtual_y_running_time_sec <= self.virtual_y_cmd_time_sec
self.stop_base_movement = not self.moving_to_new_x_pos and not self.moving_to_new_y_pos
self.vel_pub.publish(cmd_twist)
elif self.stop_base_movement:
# Once we have reached our target, we need to send a zero-velocity twist to stop it.
cmd_twist = Twist()
self.vel_pub.publish(cmd_twist)
self.stop_base_movement = False
# Just to be safe, let's reset our timers.
if not self.moving_to_new_x_pos and not self.moving_to_new_y_pos:
self.virtual_x_running_time_sec = 0.0
self.virtual_y_running_time_sec = 0.0
# Takes in a list of joint angles for joints 1-5 and publishes them for the YouBot to receive.
#
# Params:
# joint_positions - the list of joint positions to publish. Should be for arm joints 1-5.
def publish_arm_joint_positions(self, joint_positions):
desiredPositions = JointPositions()
jointCommands = []
for i in range(5):
joint = JointValue()
joint.joint_uri = "arm_joint_" + str(i+1)
joint.unit = "rad"
joint.value = joint_positions[i]
jointCommands.append(joint)
desiredPositions.positions = jointCommands
self.arm_joint_pub.publish(desiredPositions)
# Publishes the parameter gripper width to the YouBot to set its position.
#
# Params:
# width - the width value to be applied to both gripper fingers.
def publish_gripper_width(self, width):
desiredPositions = JointPositions()
jointCommands = []
joint = JointValue()
joint.joint_uri = "gripper_finger_joint_l"
joint.unit = "m"
joint.value = width
jointCommands.append(joint)
joint = JointValue()
joint.joint_uri = "gripper_finger_joint_r"
joint.unit = "m"
joint.value = width
jointCommands.append(joint)
desiredPositions.positions = jointCommands
self.gripper_pub.publish(desiredPositions)
# Goes through a routine of predefined poses that starts at a neutral position, moves down
# to grasp the grasp an object, and moves back to a neutral position.
def grasp_routine(self):
# --- Get into position for grasp --- #
pose_above_grasp = quat_pos_to_se3(quat_above_grasp, pos_above_grasp)
# Try to solve IK for the pickup point.
q_ik = self.kin_with_virtual.inverse(pose_above_grasp, jointGuessForGrasp)
if q_ik != None:
q_ik = np.array(q_ik)
# Publish the IK results.
arm_above_grasp = [q_ik[2], q_ik[3], q_ik[4], q_ik[5], q_ik[6]]
arm_above_grasp = self.limit_arm_joints(arm_above_grasp)
self.publish_arm_joint_positions(arm_above_grasp)
# Sleep for a while to give the arm time to get there.
rospy.sleep(2.0)
# --- Open grippers --- #
self.publish_gripper_width(gripperWidthOpen)
rospy.sleep(2.0)
# --- Move down to grasp --- #
# Solve IK for the grasp point:
pos_at_grasp = pos_above_grasp.copy()
pos_at_grasp[0:2] -= q_ik[0:2]
pos_at_grasp[2] -= 0.1 # move down 10 cm
pose_grasp = quat_pos_to_se3(quat_above_grasp, pos_at_grasp)
q_grasp = dls_ik(self.kin_grasp, pose_grasp, q_ik[2:])
q_grasp = self.limit_arm_joints(q_grasp)
self.publish_arm_joint_positions(q_grasp)
rospy.sleep(2.0)
# --- Close the grippers --- #
self.publish_gripper_width(gripperWidthAtGrasp)
rospy.sleep(1.5)
# --- Go to the carry position --- #
self.publish_arm_joint_positions(armJointPosCandle)
rospy.sleep(2.0)
# --- Go to dropoff position --- #
# Just use the position above grasp
self.publish_arm_joint_positions(arm_above_grasp)
# Sleep for a while to give the arm time to get there.
rospy.sleep(2.0)
# --- Open grippers --- #
self.publish_gripper_width(gripperWidthOpen)
rospy.sleep(2.0)
# --- Go to the carry position --- #
self.publish_arm_joint_positions(jointCamera)
rospy.sleep(2.0)
# --- Code for moving the base with virtual joints --- #
# Get the position to move the virtual joints to.
#self.commanded_virtual_x_pos = q_ik[0]
#self.commanded_virtual_y_pos = q_ik[1]
# Establish the velocities in the proper directions.
#if self.commanded_virtual_x_pos > 0.0:
# self.commanded_virtual_x_vel = self.max_virtual_x_vel
# self.moving_to_new_x_pos = True
# self.virtual_x_running_time_sec = 0.0
#elif self.commanded_virtual_x_pos < 0.0:
# self.commanded_virtual_x_vel = self.max_virtual_x_vel
# self.moving_to_new_x_pos = True
# self.virtual_x_running_time_sec = 0.0
#if self.commanded_virtual_y_pos > 0.0:
# self.commanded_virtual_y_vel = self.max_virtual_y_vel
# self.moving_to_new_y_pos = True
# self.virtual_y_running_time_sec = 0.0
#elif self.commanded_virtual_y_pos < 0.0:
# self.commanded_virtual_y_vel = self.max_virtual_y_vel
# self.moving_to_new_y_pos = True
# self.virtual_y_running_time_sec = 0.0
# Set up the amount of time to run the commands.
#self.virtual_x_cmd_time_sec = math.fabs(self.commanded_virtual_x_pos / self.max_virtual_x_vel)
#self.virtual_y_cmd_time_sec = math.fabs(self.commanded_virtual_y_pos / self.max_virtual_y_vel)
# --- Set the arm --- #
#arm_joints = [q_ik[2], q_ik[3], q_ik[4], q_ik[5], q_ik[6]]
#arm_joints = self.limit_arm_joints(arm_joints)
#self.publish_arm_joint_positions(arm_joints)
else:
rospy.logwarn("Invalid IK solution!")
# Clamps the parameter list of joints for joints 1-5 between their minimum and maximum values.
#
# Params:
# joints - the list of joint angles to limit. Should contain joints 1-5.
#
# Returns:
# The list of limited joint values.
def limit_arm_joints(self, joints):
for i in range(5):
joints[i] = low_high_limit(joints[i], jointMin[i], jointMax[i])
return joints
class KinematicsTest(object):
"""Test kinematics using KDL"""
def __init__(self):
"""Read robot describtion"""
self.robot_ = URDF.from_parameter_server()
self.kdl_kin_ = KDLKinematics(self.robot_, 'base_link', 'end_effector')
self.cur_pose_ = None
self.cur_jnt_ = None
# Creates the SimpleActionClient, passing the type of the action
self.client_ = actionlib.SimpleActionClient('/mitsubishi_arm/mitsubishi_trajectory_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction)
self.client_.wait_for_server()
self.goal_ = control_msgs.msg.FollowJointTrajectoryGoal()
#time_from_start=rospy.Duration(10)
self.goal_.trajectory.joint_names=['j1','j2','j3','j4','j5','j6']
#To be reached 1 second after starting along the trajectory
trajectory_point=trajectory_msgs.msg.JointTrajectoryPoint()
trajectory_point.positions=[0]*6
trajectory_point.time_from_start=rospy.Duration(0.1)
self.goal_.trajectory.points.append(trajectory_point)
rospy.Subscriber('joint_states', sensor_msgs.msg.JointState, self.fwd_kinematics_cb)
rospy.Subscriber('command', geometry_msgs.msg.Pose, self.inv_kinematics_cb)
def fwd_kinematics_cb(self, data):
"""Compute forward kinematics and print it out
:data: TODO
:returns: TODO
"""
self.cur_jnt_ = data.position
self.cur_pose_ = self.kdl_kin_.forward(data.position)
q = np.array(data.position)
new_pose = self.cur_pose_.copy()
new_pose[0,3] += 0.01
#print "cp:", np.ravel(self.cur_pose_[:3,3])
#print "np:", np.ravel(new_pose[:3,3])
#q_ik = self.kdl_kin_.inverse(new_pose, q)
#print "Q:", q
#print "Q_ik:", q_ik
def inv_kinematics_cb(self, data):
"""calculate the ik given a pose (only xyz right now)
:data: TODO
:returns: TODO
"""
# preserve the rotation
new_pose = self.cur_pose_.copy()
new_pose[0,3] = data.position.x
new_pose[1,3] = data.position.y
new_pose[2,3] = data.position.z
q_ik = self.kdl_kin_.inverse(new_pose, self.cur_jnt_)
print "cur:", np.ravel(self.cur_pose_[:3,3])
print "req:", np.ravel(new_pose[:3,3])
print "Q:", self.cur_jnt_
print "Q_ik:", q_ik
if q_ik is not None:
print "Sending goal"
self.goal_.trajectory.points[0].positions = q_ik
self.client_.send_goal(self.goal_)
def __init__(self):
rospy.init_node('ur_simulation',anonymous=True)
rospy.logwarn('SIMPLE_UR SIMULATION DRIVER LOADING')
# TF
self.broadcaster_ = tf.TransformBroadcaster()
self.listener_ = tf.TransformListener()
# SERVICES
self.servo_to_pose_service = rospy.Service('simple_ur_msgs/ServoToPose', ServoToPose, self.servo_to_pose_call)
self.set_stop_service = rospy.Service('simple_ur_msgs/SetStop', SetStop, self.set_stop_call)
self.set_teach_mode_service = rospy.Service('simple_ur_msgs/SetTeachMode', SetTeachMode, self.set_teach_mode_call)
self.set_servo_mode_service = rospy.Service('simple_ur_msgs/SetServoMode', SetServoMode, self.set_servo_mode_call)
# PUBLISHERS AND SUBSCRIBERS
self.driver_status_publisher = rospy.Publisher('/ur_robot/driver_status',String)
self.robot_state_publisher = rospy.Publisher('/ur_robot/robot_state',String)
self.joint_state_publisher = rospy.Publisher('joint_states',JointState)
# self.follow_pose_subscriber = rospy.Subscriber('/ur_robot/follow_goal',PoseStamped,self.follow_goal_cb)
# Predicator
self.pub_list = rospy.Publisher('/predicator/input', PredicateList)
self.pub_valid = rospy.Publisher('/predicator/valid_input', ValidPredicates)
rospy.sleep(1)
pval = ValidPredicates()
pval.pheader.source = rospy.get_name()
pval.predicates = ['moving', 'stopped', 'running']
pval.assignments = ['robot']
self.pub_valid.publish(pval)
# Rate
self.run_rate = rospy.Rate(100)
self.run_rate.sleep()
### Set Up Simulated Robot ###
self.driver_status = 'IDLE'
self.robot_state = 'POWER OFF'
robot = URDF.from_parameter_server()
self.kdl_kin = KDLKinematics(robot, 'base_link', 'ee_link')
# self.q = self.kdl_kin.random_joint_angles()
self.q = [-1.5707,-.785,-3.1415+.785,-1.5707-.785,-1.5707,0] # Start Pose?
self.start_pose = self.kdl_kin.forward(self.q)
self.F_start = tf_c.fromMatrix(self.start_pose)
# rospy.logwarn(self.start_pose)
# rospy.logwarn(type(self.start_pose))
# pose = self.kdl_kin.forward(q)
# joint_positions = self.kdl_kin.inverse(pose, q+0.3) # inverse kinematics
# if joint_positions is not None:
# pose_sol = self.kdl_kin.forward(joint_positions) # should equal pose
# J = self.kdl_kin.jacobian(q)
# rospy.logwarn('q:'+str(q))
# rospy.logwarn('joint_positions:'+str(joint_positions))
# rospy.logwarn('pose:'+str(pose))
# if joint_positions is not None:
# rospy.logwarn('pose_sol:'+str(pose_sol))
# rospy.logwarn('J:'+str(J))
### START LOOP ###
while not rospy.is_shutdown():
if self.driver_status == 'TEACH':
self.update_from_marker()
# if self.driver_status == 'SERVO':
# self.update_follow()
# Publish and Sleep
self.publish_status()
self.send_command()
self.run_rate.sleep()
# Finish
rospy.logwarn('SIMPLE UR - Simulation Finished')
def linearSpace(plot, T, N, vy, vz, jerk):
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
# Create seed with current position
q0 = kdl_kin.random_joint_angles()
limb_interface = baxter_interface.limb.Limb('right')
current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
for ind in range(len(q0)):
q0[ind] = current_angles[ind]
# q_start = np.array([0.2339320701525256, -0.5878981369570848, 0.19903400722813244, 1.8561167533413507,
# -0.4908738521233324, -0.97752925707998, -0.49547579448698864])
# q_throw = np.array([0.9265243958827899, -0.7827136970185323, -0.095490304045867, 1.8338740319170121,
# -0.03681553890924993, -0.9909515889739773, -0.5840631849873713])
# q_end = np.array([0.9085001216251363, -1.0089758632316308, 0.07401457301547121, 1.8768254939778037,
# 0.18599517053110642, -0.8172282647459542, -0.44600491407768406])
q_throw = np.array([ 0.47668453, -0.77274282, 0.93150983, 2.08352941, 0.54149522,
-1.26745163, -2.06742261])
q_end = np.array([ 0.75356806, -0.89162633, 0.54648066, 2.08698086, 0.41033986,
-1.18423317, -1.15815549])
q_start = np.array([-0.22281071, -0.36470393, 0.36163597, 1.71920897, -0.82719914,
-1.16889336, -0.90888362])
X_start = np.asarray(kdl_kin.forward(q_start))
X_throw = np.asarray(kdl_kin.forward(q_throw))
# offset throw position
X_throw[0,3] += 0
X_throw[1,3] += 0
X_throw[2,3] += 0
X_end = np.asarray(kdl_kin.forward(q_end))
Vb = np.array([0,0,0,0,vy,vz])
# xStart = X_start
# xEnd = X_throw
vStart = np.array([0,0,0,0,0,0])
vEnd = Vb
aStart = np.array([0,0,0,0,0,0])
aEnd = np.array([0,0,0,0,0,0])
jStart = np.array([0,0,0,0,0,0])
jEnd = np.array([0,0,0,0,0,jerk])
# T = .7
# N = 200*T
a = np.array([[1,0,0,0,0,0,0,0],[1,T,T**2,T**3,T**4,T**5,T**6,T**7],[0,1,0,0,0,0,0,0],[0,1,2*T,3*T**2,4*T**3,5*T**4,6*T**5,7*T**6],
[0,0,2,0,0,0,0,0],[0,0,2,6*T,12*T**2,20*T**3,30*T**4,42*T**5],[0,0,0,6,0,0,0,0],[0,0,0,6,24*T,60*T**2,120*T**3,210*T**4]])
tSpace = np.linspace(0,T,N);
tOffset = T
colors = ['r','b','c','y','m','oc','k']
if plot == True:
plt.close('all')
pLista = np.empty((3,N))
pListb = np.empty((3,N))
vLista = np.empty((3,N))
vListb = np.empty((3,N))
aLista = np.empty((3,N))
aListb = np.empty((3,N))
for i in range(3):
b = np.array([X_start[i,3],X_throw[i,3],0,vEnd[3+i],0,0,jStart[i+3],jEnd[i+3]])
coeff = np.linalg.solve(a,b)
j1Pa = jointPath(tSpace,coeff)
j1Va = jointVelocity(tSpace,coeff)
j1Aa = jointAcceleration(tSpace,coeff)
b = np.array([X_throw[i,3],X_end[i,3],0,0,0,0,jEnd[i+3],jStart[i+3]])
# b = np.array([X_throw[i,3],X_end[i,3],vEnd[3+i],0,0,0,jEnd[i+3],jStart[i+3]])
coeff = np.linalg.solve(a,b)
j1Pb = jointPath(tSpace,coeff)
j1Vb = jointVelocity(tSpace,coeff)
j1Ab = jointAcceleration(tSpace,coeff)
pLista[i,:] = j1Pa
pListb[i,:] = j1Pb
vLista[i,:] = j1Va
vListb[i,:] = j1Vb
aLista[i,:] = j1Aa
aListb[i,:] = j1Ab
vList = np.hstack((vLista,vListb)).transpose()
dt = float(T)/(N-1)
if plot == True:
plt.figure()
plt.title('Position (cartesian)')
plt.plot(np.hstack((pLista,pListb)).transpose())
plt.figure()
plt.title('Velocity (cartesian)')
plt.plot(np.hstack((vLista,vListb)).transpose())
# plt.figure()
# plt.title('Velocity dt')
# vList = np.diff(pLista)/dt
# plt.plot(vList.transpose())
plt.figure()
plt.title('Acceleration (cartesian)')
plt.plot(np.hstack((aLista,aListb)).transpose())
plt.show(block=False)
Xlista = np.empty((N,4,4))
Xlistb = np.empty((N,4,4))
Re, pe = TransToRp(X_throw)
i=0
for t in tSpace[0:]:
pa = pLista[:,i]
Ra = Re
Xlista[i] = RpToTrans(Ra,pa)
pb = pListb[:,i]
Rb = Re
Xlistb[i] = RpToTrans(Rb,pb)
i = i + 1
XlistT = np.vstack((Xlista,Xlistb[1:,:,:]))
q0 = kdl_kin.random_joint_angles()
current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
for ind in range(len(q0)):
q0[ind] = q_start[ind]
thList = np.empty((N*2-1,7))
thList[0] = q0;
for i in range(int(2*N)-2):
# Solve for joint angles
seed = 0
q_ik = kdl_kin.inverse(XlistT[i+1], thList[i])
while q_ik == None:
seed += 0.03
q_ik = kdl_kin.inverse(XlistT[i+1], thList[i]+seed)
if (seed>1):
q_ik = thList[i]
break
thList[i+1] = q_ik
thList = thList[1:,:]
# if plot == True:
# plt.figure()
# plt.plot(thList)
# plt.show(block=False)
jList = np.empty((thList.shape[0],7))
# Compare jacobian velocities to joint velocities
for i in range(thList.shape[0]):
jacobian = kdl_kin.jacobian(thList[i])
inv_jac = np.linalg.pinv(jacobian)
Vb = np.hstack((np.array([0,0,0]), vList[i]))
q_dot_throw = inv_jac.dot(Vb)
jList[i,:] = q_dot_throw
# plt.figure()
# plt.title('Jacobian-based joint velocities')
# plt.plot(jList);
# plt.show(block = False)
vList = np.diff(thList,axis=0)/dt
vList = np.vstack((np.array([0,0,0,0,0,0,0]),vList))
vCarlist = np.empty((thList.shape[0],6))
for i in range(thList.shape[0]):
jacobian = kdl_kin.jacobian(thList[i])
vCar = jacobian.dot(vList[i]);
vCarlist[i,:] = vCar;
if (plot==True):
plt.figure()
plt.title('Jacobian based cartesian velocities')
plt.plot(vCarlist[:,0:3])
plt.show(block=False)
if (plot==True):
plt.figure()
plt.title('Joint velocities')
plt.plot(vList)
plt.show(block=False)
return (thList, jList)
class HebiArmController(object):
def __init__(self, hebi_group_name, hebi_mapping, hebi_gains, urdf_str, base_link, end_link):
rospy.loginfo("Creating {} instance".format(self.__class__.__name__))
self.openmeta_testbench_manifest_path = rospy.get_param('~openmeta/testbench_manifest_path', None)
if self.openmeta_testbench_manifest_path is not None:
self._testbench_manifest = load_json_file(self.openmeta_testbench_manifest_path)
# TestBench Parameters
self._params = {}
for tb_param in self._testbench_manifest['Parameters']:
self._params[tb_param['Name']] = tb_param['Value'] # WARNING: If you use these values - make sure to check the type
# TestBench Metrics
self._metrics = {}
for tb_metric in self._testbench_manifest['Metrics']: # FIXME: Hmm, this is starting to look a lot like OpenMDAO...
self._metrics[tb_metric['Name']] = tb_metric['Value']
if hebi_gains is None:
hebi_gains = {
'p': [float(self._params['p_gain'])]*3,
'i': [float(self._params['i_gain'])]*3,
'd': [float(self._params['d_gain'])]*3
}
hebi_families = []
hebi_names = []
for actuator in hebi_mapping:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_wrap = HebirosWrapper(hebi_group_name, hebi_families, hebi_names)
# Set PID Gains
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping
cmd_msg.settings.name = hebi_mapping
cmd_msg.settings.control_strategy = [4, 4, 4]
cmd_msg.settings.position_gains.name = hebi_mapping
cmd_msg.settings.position_gains.kp = hebi_gains['p']
cmd_msg.settings.position_gains.ki = hebi_gains['i']
cmd_msg.settings.position_gains.kd = hebi_gains['d']
cmd_msg.settings.position_gains.i_clamp = [0.25, 0.25, 0.25] # TODO: Tune me.
self.hebi_wrap.send_command_with_acknowledgement(cmd_msg)
if base_link is None or end_link is None:
robot = Robot.from_xml_string(urdf_str)
if not base_link:
base_link = robot.get_root()
# WARNING: There may be multiple leaf nodes
if not end_link:
end_link = [x for x in robot.link_map.keys() if x not in robot.child_map.keys()][0]
# pykdl
self.kdl_fk = KDLKinematics(URDF.from_xml_string(urdf_str), base_link, end_link)
self._active_joints = self.kdl_fk.get_joint_names()
# joint data
self.position_fbk = [0.0]*self.hebi_wrap.hebi_count
self.velocity_fbk = [0.0]*self.hebi_wrap.hebi_count
self.effort_fbk = [0.0]*self.hebi_wrap.hebi_count
# joint state publisher
while not rospy.is_shutdown() and len(self.hebi_wrap.get_joint_positions()) < len(self.hebi_wrap.hebi_mapping):
rospy.sleep(0.1)
self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self.hebi_wrap.add_feedback_callback(self._joint_state_cb)
# Set up Waypoint/Trajectory objects
self.start_wp = WaypointMsg()
self.start_wp.names = self.hebi_wrap.hebi_mapping
self.end_wp = copy.deepcopy(self.start_wp)
self.goal = TrajectoryGoal()
self.goal.waypoints = [self.start_wp, self.end_wp]
self._hold_positions = [0.0]*3
self._hold = True
self.jointstate = JointState()
self.jointstate.name = self.hebi_wrap.hebi_mapping
rospy.sleep(1.0)
def execute(self, rate, joint_states, durations):
max_height = 0.0
while not rospy.is_shutdown():
if len(joint_states) > 0:
# start waypoint
self.start_wp.positions = self.position_fbk
self.start_wp.velocities = [0.0]*self.hebi_wrap.hebi_count
self.start_wp.accelerations = [0.0]*self.hebi_wrap.hebi_count
# end waypoint
self.end_wp.positions = joint_states.pop(0)
self.end_wp.velocities = [0.0]*self.hebi_wrap.hebi_count
self.end_wp.accelerations = [0.0]*self.hebi_wrap.hebi_count
self._hold_positions = self.end_wp.positions
# action goal
self.goal.times = [0.0, durations.pop()]
self._hold = False
self.hebi_wrap.trajectory_action_client.send_goal(self.goal)
self.hebi_wrap.trajectory_action_client.wait_for_result()
self._hold = True
# check status
if abs(self.position_fbk[0] - self.end_wp.positions[0]) > 0.0872665:
print("Failed to achieve objective position.")
break
else:
rospy.sleep(2.0)
fk = self.kdl_fk.forward(self.position_fbk)
max_height = fk.tolist()[2][3]
print("Max Height: {}".format(max_height))
break
rate.sleep()
if self.openmeta_testbench_manifest_path is not None:
steps_completed = 0
if 7 <= len(joint_states):
steps_completed = 0
elif 5 <= len(joint_states) < 7:
steps_completed = 1
elif 3 <= len(joint_states) < 5:
steps_completed = 2
elif len(joint_states) < 3:
steps_completed = 3
self._metrics['steps_completed'] = steps_completed
self._metrics['max_height'] = max_height
self._write_metrics_to_tb_manifest()
def _joint_state_cb(self, msg):
if not rospy.is_shutdown():
if self._hold:
self.jointstate.position = self._hold_positions
self.jointstate.velocity = []
self.hebi_wrap.joint_state_publisher.publish(self.jointstate)
self.position_fbk = self.hebi_wrap.get_joint_positions()
self.velocity_fbk = self.hebi_wrap.get_joint_velocities()
self.effort_fbk = self.hebi_wrap.get_joint_efforts()
jointstate = JointState()
jointstate.header.stamp = rospy.Time.now()
jointstate.name = self._active_joints
jointstate.position = self.position_fbk
jointstate.velocity = self.velocity_fbk
jointstate.effort = self.effort_fbk
self._joint_state_pub.publish(jointstate)
def _write_metrics_to_tb_manifest(self):
# Write to testbench_manifest metric
for tb_metric in self._testbench_manifest['Metrics']:
if tb_metric['Name'] in self._metrics:
tb_metric['Value'] = self._metrics[tb_metric['Name']]
# Save updated testbench_manifest.json
with open(self.openmeta_testbench_manifest_path, 'w') as savefile:
json_str = json.dumps(self._testbench_manifest, sort_keys=True, indent=2, separators=(',', ': '))
savefile.write(json_str)
def get_jacabian_from_joint(self, urdfname, jointq, flag):
robot = URDF.from_xml_file(urdfname)
kdl_kin = KDLKinematics(robot, "base_link", "tool0")
J = kdl_kin.jacobian(jointq)
pose = kdl_kin.forward(jointq)
return J, pose
class CostarArm(object):
def __init__(self,
base_link,
end_link,
planning_group,
world="/world",
listener=None,
broadcaster=None,
traj_step_t=0.1,
max_acc=1,
max_vel=1,
max_goal_diff=0.02,
goal_rotation_weight=0.01,
max_q_diff=1e-6,
start_js_cb=True,
base_steps=10,
steps_per_meter=100,
closed_form_IK_solver=None,
dof=7,
perception_ns="/SPServer"):
self.world = world
self.base_link = base_link
self.end_link = end_link
self.planning_group = planning_group
self.dof = dof
self.base_steps = base_steps
self.steps_per_meter = steps_per_meter
self.MAX_ACC = max_acc
self.MAX_VEL = max_vel
self.traj_step_t = traj_step_t
self.max_goal_diff = max_goal_diff
self.max_q_diff = max_q_diff
self.goal_rotation_weight = goal_rotation_weight
self.at_goal = True
self.near_goal = True
self.moving = False
self.q0 = None #[0] * self.dof
self.old_q0 = [0] * self.dof
self.cur_stamp = 0
# Set up TF broadcaster
if not broadcaster is None:
self.broadcaster = broadcaster
else:
self.broadcaster = tf.TransformBroadcaster()
# Set up TF listener and smartmove manager
if listener is None:
self.listener = tf.TransformListener()
else:
self.listener = listener
# Currently this class does not need a smart waypoint manager.
# That will remain in the CoSTAR BT.
#self.smartmove_manager = SmartWaypointManager(
# listener=self.listener,
# broadcaster=self.broadcaster)
# TODO: ensure the manager is set up properly
# Note that while the waypoint manager is currently a part of CostarArm
# If we wanted to set this up for multiple robots it should be treated
# as an independent component.
self.waypoint_manager = WaypointManager(service=True,
broadcaster=self.broadcaster)
# Set up services
# The CostarArm services let the UI put it into teach mode or anything else
self.teach_mode = rospy.Service('/costar/SetTeachMode', SetTeachMode,
self.set_teach_mode_call)
self.servo_mode = rospy.Service('/costar/SetServoMode', SetServoMode,
self.set_servo_mode_call)
self.shutdown = rospy.Service('/costar/ShutdownArm', EmptyService,
self.shutdown_arm_call)
self.servo = rospy.Service('/costar/ServoToPose', ServoToPose,
self.servo_to_pose_call)
self.plan = rospy.Service('/costar/PlanToPose', ServoToPose,
self.plan_to_pose_call)
self.smartmove = rospy.Service('/costar/SmartMove', SmartMove,
self.smart_move_call)
self.js_servo = rospy.Service('/costar/ServoToJointState',
ServoToJointState,
self.servo_to_joints_call)
self.save_frame = rospy.Service('/costar/SaveFrame', SaveFrame,
self.save_frame_call)
self.save_joints = rospy.Service('/costar/SaveJointPosition',
SaveFrame, self.save_joints_call)
self.get_waypoints_srv = GetWaypointsService(world=world,
service=False,
ns=perception_ns)
self.driver_status = 'IDLE'
# Create publishers. These will send necessary information out about the state of the robot.
self.pt_publisher = rospy.Publisher('/joint_traj_pt_cmd',
JointTrajectoryPoint,
queue_size=1000)
self.status_publisher = rospy.Publisher('/costar/DriverStatus',
String,
queue_size=1000)
self.display_pub = rospy.Publisher('costar/display_trajectory',
DisplayTrajectory,
queue_size=1000)
self.robot = URDF.from_parameter_server()
if start_js_cb:
self.js_subscriber = rospy.Subscriber('joint_states', JointState,
self.js_cb)
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(base_link, end_link)
# cCreate reference to pyKDL kinematics
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
#self.set_goal(self.q0)
self.goal = None
self.ee_pose = None
self.joint_names = [
joint.name for joint in self.robot.joints[:self.dof]
]
self.planner = SimplePlanning(
self.robot,
base_link,
end_link,
self.planning_group,
kdl_kin=self.kdl_kin,
joint_names=self.joint_names,
closed_form_IK_solver=closed_form_IK_solver)
'''
js_cb
listen to robot joint state information
'''
def js_cb(self, msg):
if len(msg.position) is self.dof:
pass
self.old_q0 = self.q0
self.q0 = np.array(msg.position)
else:
rospy.logwarn('Incorrect joint dimensionality')
'''
update current position information
'''
def update_position(self):
if self.q0 is None or self.old_q0 is None:
return
self.ee_pose = pm.fromMatrix(self.kdl_kin.forward(self.q0))
if self.goal is not None:
#goal_diff = np.abs(self.goal - self.q0).sum() / self.q0.shape[0]
cart_diff = 0 #(self.ee_pose.p - self.goal.p).Norm()
rot_diff = 0 #self.goal_rotation_weight * (pm.Vector(*self.ee_pose.M.GetRPY()) - pm.Vector(*self.goal.M.GetRPY())).Norm()
goal_diff = cart_diff + rot_diff
if goal_diff < self.max_goal_diff:
self.at_goal = True
if goal_diff < 10 * self.max_goal_diff:
self.near_goal = True
q_diff = np.abs(self.old_q0 - self.q0).sum()
if q_diff < self.max_q_diff:
self.moving = False
else:
self.moving = True
def check_req_speed_params(self, req):
if req.accel > self.MAX_ACC:
acceleration = self.MAX_ACC
else:
acceleration = req.accel
if req.vel > self.MAX_VEL:
velocity = self.MAX_VEL
else:
velocity = req.vel
return (acceleration, velocity)
'''
Save the current end effector pose as a frame that we can return to
'''
def save_frame_call(self, req):
rospy.logwarn(
'Save frame does not check to see if your frame already exists!')
print self.ee_pose
self.waypoint_manager.save_frame(self.ee_pose, self.world)
return 'SUCCESS - '
'''
Save the current joint states as a frame that we can return to
'''
def save_joints_call(self, req):
rospy.logwarn(
'Save frame does not check to see if your joint position already exists!'
)
print self.q0
self.waypoint_manager.save_frame(self.q0)
return 'SUCCESS - '
'''
Find any valid object that meets the requirements.
Find a cartesian path or possibly longer path through joint space.
'''
def smart_move_call(self, req):
if not self.driver_status == 'SERVO':
rospy.logerr('DRIVER -- Not in servo mode!')
return 'FAILURE - not in servo mode'
# Check acceleration and velocity limits
(acceleration, velocity) = self.check_req_speed_params(req)
# Find possible poses
res = self.get_waypoints_srv.get_waypoints(
req.obj_class, # object class to move to
req.predicates, # predicates to match
[req.pose], # offset/transform from each member of the class
[req.name] # placeholder name
)
if res is None:
msg = 'FAILURE - no objects found that meet predicate conditions!'
return msg
(poses, names, objects) = res
T_fwd = pm.fromMatrix(self.kdl_kin.forward(self.q0))
if not poses is None:
msg = 'FAILURE - no valid objects found!'
dists = []
Ts = []
for (pose, name, obj) in zip(poses, names, objects):
# figure out which tf frame we care about
tf_path = name.split('/')
tf_frame = ''
for part in tf_path:
if len(part) > 0:
tf_frame = part
break
if len(tf_frame) == 0:
continue
# try to move to the pose until one succeeds
T_base_world = pm.fromTf(
self.listener.lookupTransform(self.world, self.base_link,
rospy.Time(0)))
T = T_base_world.Inverse() * pm.fromMsg(pose)
Ts.append(T)
# TODO(cpaxton) update this to include rotation
dists.append((T.p - T_fwd.p).Norm())
if len(Ts) == 0:
msg = 'FAILURE - no objects found!'
else:
possible_goals = zip(dists, Ts, objects, names)
possible_goals.sort()
for (dist, T, obj, name) in possible_goals:
rospy.logwarn("Trying to move to frame at distance %f" %
(dist))
# plan to T
(code, res) = self.planner.getPlan(T, self.q0, obj=obj)
msg = self.send_and_publish_planning_result(
res, acceleration, velocity)
if msg[0:7] == 'SUCCESS':
break
return msg
else:
msg = 'FAILURE - no matching moves for specified predicates'
return msg
def set_goal(self, q):
self.at_goal = False
self.near_goal = False
self.goal = pm.fromMatrix(self.kdl_kin.forward(q))
def send_and_publish_planning_result(self, res, acceleration, velocity):
if (not res is None) and len(
res.planned_trajectory.joint_trajectory.points) > 0:
disp = DisplayTrajectory()
disp.trajectory.append(res.planned_trajectory)
disp.trajectory_start = res.trajectory_start
self.display_pub.publish(disp)
traj = res.planned_trajectory.joint_trajectory
return self.send_trajectory(traj,
acceleration,
velocity,
cartesian=False)
else:
rospy.logerr('DRIVER -- PLANNING failed')
return 'FAILURE - not in servo mode'
'''
Definitely do a planned motion.
'''
def plan_to_pose_call(self, req):
#rospy.loginfo('Recieved servo to pose request')
#print req
if self.driver_status == 'SERVO':
T = pm.fromMsg(req.target)
# Check acceleration and velocity limits
(acceleration, velocity) = self.check_req_speed_params(req)
# Send command
pt = JointTrajectoryPoint()
pose = pm.fromMsg(req.target)
(code, res) = self.planner.getPlan(pose, self.q0)
print "DONE PLANNING: " + str((code, res))
return self.send_and_publish_planning_result(
res, acceleration, velocity)
else:
rospy.logerr('DRIVER -- not in servo mode!')
return 'FAILURE - not in servo mode'
'''
Send a whole joint trajectory message to a robot...
that is listening to individual joint states.
'''
def send_trajectory(self,
traj,
acceleration=0.5,
velocity=0.5,
cartesian=False):
rospy.logerr(
"Function 'send_trajectory' not implemented for base class!")
return "FAILURE - running base class!"
'''
Standard movement call.
Tries a cartesian move, then if that fails goes into a joint-space move.
'''
def servo_to_pose_call(self, req):
if self.driver_status == 'SERVO':
T = pm.fromMsg(req.target)
# Check acceleration and velocity limits
(acceleration, velocity) = self.check_req_speed_params(req)
# inverse kinematics
traj = self.planner.getCartesianMove(T, self.q0, self.base_steps,
self.steps_per_meter)
#if len(traj.points) == 0:
# (code,res) = self.planner.getPlan(req.target,self.q0) # find a non-local movement
# if not res is None:
# traj = res.planned_trajectory.joint_trajectory
# Send command
if len(traj.points) > 0:
rospy.logwarn("Robot moving to " +
str(traj.points[-1].positions))
return self.send_trajectory(traj,
acceleration,
velocity,
cartesian=False,
linear=True)
else:
rospy.logerr('SIMPLE DRIVER -- IK failed')
return 'FAILURE - not in servo mode'
else:
rospy.logerr('SIMPLE DRIVER -- Not in servo mode')
return 'FAILURE - not in servo mode'
'''
Standard move call.
Make a joint space move to a destination.
'''
def servo_to_joints_call(self, req):
if self.driver_status == 'SERVO':
# Check acceleration and velocity limits
(acceleration, velocity) = self.check_req_speed_params(req)
return 'FAILURE - not yet implemented!'
else:
rospy.logerr('SIMPLE DRIVER -- Not in servo mode')
return 'FAILURE - not in servo mode'
'''
set teach mode
'''
def set_teach_mode_call(self, req):
if req.enable == True:
# self.rob.set_freedrive(True)
self.driver_status = 'TEACH'
return 'SUCCESS - teach mode enabled'
else:
# self.rob.set_freedrive(False)
self.driver_status = 'IDLE'
return 'SUCCESS - teach mode disabled'
'''
send a single point
'''
def send_q(self, pt, acceleration, velocity):
pt = JointTrajectoryPoint()
pt.positions = self.q0
self.pt_publisher.publish(pt)
'''
activate servo mode
'''
def set_servo_mode_call(self, req):
if req.mode == 'SERVO':
self.send_q(self.q0, 0.1, 0.1)
self.driver_status = 'SERVO'
return 'SUCCESS - servo mode enabled'
elif req.mode == 'DISABLE':
self.driver_status = 'IDLE'
return 'SUCCESS - servo mode disabled'
def shutdown_arm_call(self, req):
self.driver_status = 'SHUTDOWN'
pass
'''
robot-specific logic to update state every "tick"
'''
def handle_tick(self):
rospy.logerr("Function 'handle_tick' not implemented for base class!")
'''
call this when "spinning" to keep updating things
'''
def tick(self):
self.status_publisher.publish(self.driver_status)
self.update_position()
self.handle_tick()
# publish TF messages to display frames
self.waypoint_manager.publish_tf()
class YumiGelslimPybullet(object):
"""
Class for interfacing with Yumi in PyBullet
with external motion planning, inverse kinematics,
and forward kinematics, along with other helpers
"""
def __init__(self, yumi_pb, cfg, exec_thread=True, sim_step_repeat=10):
"""
Class constructor. Sets up internal motion planning interface
for each arm, forward and inverse kinematics solvers, and background
threads for updating the position of the robot.
Args:
yumi_pb (airobot Robot): Instance of PyBullet simulated robot, from
airobot library
cfg (YACS CfgNode): Configuration parameters
exec_thread (bool, optional): Whether or not to start the
background joint position control thread. Defaults to True.
sim_step_repeat (int, optional): Number of simulation steps
to take each time the desired joint position value is
updated. Defaults to 10
"""
self.cfg = cfg
self.yumi_pb = yumi_pb
self.sim_step_repeat = sim_step_repeat
self.joint_lock = threading.RLock()
self._both_pos = self.yumi_pb.arm.get_jpos()
self._single_pos = {}
self._single_pos['right'] = \
self.yumi_pb.arm.arms['right'].get_jpos()
self._single_pos['left'] = \
self.yumi_pb.arm.arms['left'].get_jpos()
self.moveit_robot = moveit_commander.RobotCommander()
self.moveit_scene = moveit_commander.PlanningSceneInterface()
self.moveit_planner = 'RRTConnectkConfigDefault'
self.robot_description = '/robot_description'
self.urdf_string = rospy.get_param(self.robot_description)
self.mp_left = GroupPlanner('left_arm',
self.moveit_robot,
self.moveit_planner,
self.moveit_scene,
max_attempts=3,
planning_time=5.0,
goal_tol=0.5,
eef_delta=0.01,
jump_thresh=10)
self.mp_right = GroupPlanner('right_arm',
self.moveit_robot,
self.moveit_planner,
self.moveit_scene,
max_attempts=3,
planning_time=5.0,
goal_tol=0.5,
eef_delta=0.01,
jump_thresh=10)
self.fk_solver_r = KDLKinematics(URDF.from_parameter_server(),
"yumi_body", "yumi_tip_r")
self.fk_solver_l = KDLKinematics(URDF.from_parameter_server(),
"yumi_body", "yumi_tip_l")
self.num_ik_solver_r = trac_ik.IK('yumi_body',
'yumi_tip_r',
urdf_string=self.urdf_string)
self.num_ik_solver_l = trac_ik.IK('yumi_body',
'yumi_tip_l',
urdf_string=self.urdf_string)
self.ik_pos_tol = 0.001 # 0.001 working well with pulling
self.ik_ori_tol = 0.01 # 0.01 working well with pulling
self.execute_thread = threading.Thread(target=self._execute_both)
self.execute_thread.daemon = True
if exec_thread:
self.execute_thread.start()
self.step_sim_mode = False
else:
self.step_sim_mode = True
def _execute_single(self):
"""
Background thread for controlling a single arm
"""
while True:
self.joint_lock.acquire()
self.yumi_pb.arm.set_jpos(self._both_pos, wait=True)
self.joint_lock.release()
time.sleep(0.01)
def _execute_both(self):
"""
Background thread for controlling both arms
"""
while True:
self.joint_lock.acquire()
self.yumi_pb.arm.set_jpos(self._both_pos, wait=True)
self.joint_lock.release()
time.sleep(0.01)
def update_joints(self, pos, arm=None):
"""
Setter function for external user to update the target
joint values for the arms. If manual step mode is on,
this function also takes simulation steps.
Args:
pos (list): Desired joint angles, either for both arms or
a single arm
arm (str, optional): Which arm to update the joint values for
either 'right', or 'left'. If none, assumed updating for
both. Defaults to None.
Raises:
ValueError: Bad arm name
"""
if arm is None:
self.joint_lock.acquire()
self._both_pos = pos
self.joint_lock.release()
elif arm == 'right':
both_pos = list(pos) + self.yumi_pb.arm.arms['left'].get_jpos()
self.joint_lock.acquire()
self._both_pos = both_pos
self.joint_lock.release()
elif arm == 'left':
both_pos = self.yumi_pb.arm.arms['right'].get_jpos() + list(pos)
self.joint_lock.acquire()
self._both_pos = both_pos
self.joint_lock.release()
else:
raise ValueError('Arm not recognized')
self.yumi_pb.arm.set_jpos(self._both_pos, wait=False)
if self.step_sim_mode:
for _ in range(self.sim_step_repeat):
# step_simulation()
self.yumi_pb.pb_client.stepSimulation()
def compute_fk(self, joints, arm='right'):
"""
Forward kinematics calculation.
Args:
joints (list): Joint configuration, should be len() =
DOF of a single arm (7 for Yumi)
arm (str, optional): Which arm to compute FK for.
Defaults to 'right'.
Returns:
PoseStamped: End effector pose corresponding to
the FK solution for the input joint configuation
"""
if arm == 'right':
matrix = self.fk_solver_r.forward(joints)
else:
matrix = self.fk_solver_l.forward(joints)
translation = transformations.translation_from_matrix(matrix)
quat = transformations.quaternion_from_matrix(matrix)
ee_pose_array = np.hstack((translation, quat))
ee_pose = util.convert_pose_type(ee_pose_array,
type_out='PoseStamped',
frame_out='yumi_body')
return ee_pose
def compute_ik(self, pos, ori, seed, arm='right', solver='trac'):
"""
Inverse kinematics calcuation
Args:
pos (list): Desired end effector position [x, y, z]
ori (list): Desired end effector orientation (quaternion),
[x, y, z, w]
seed (list): Initial solution guess to IK calculation.
Returned solution will be near the seed.
arm (str, optional): Which arm to compute IK for.
Defaults to 'right'.
solver (str, optional): Which IK solver to use.
Defaults to 'trac'.
Returns:
list: Configuration space solution corresponding to the
desired end effector pose. len() = DOF of single arm
"""
if arm != 'right' and arm != 'left':
arm = 'right'
if arm == 'right':
if solver == 'trac':
sol = self.num_ik_solver_r.get_ik(
seed, pos[0], pos[1], pos[2], ori[0], ori[1], ori[2],
ori[3], self.ik_pos_tol, self.ik_pos_tol, self.ik_pos_tol,
self.ik_ori_tol, self.ik_ori_tol, self.ik_ori_tol)
else:
sol = self.yumi_pb.arm.compute_ik(pos, ori, arm='right')
elif arm == 'left':
if solver == 'trac':
sol = self.num_ik_solver_l.get_ik(
seed, pos[0], pos[1], pos[2], ori[0], ori[1], ori[2],
ori[3], self.ik_pos_tol, self.ik_pos_tol, self.ik_pos_tol,
self.ik_ori_tol, self.ik_ori_tol, self.ik_ori_tol)
else:
sol = self.yumi_pb.arm.compute_ik(pos, ori, arm='left')
return sol
def unify_arm_trajectories(self, left_arm, right_arm, tip_poses):
"""
Function to return a right arm and left arm trajectory
of the same number of points, where the index of the points
that align with the goal cartesian poses of each arm are the
same for both trajectories
Args:
left_arm (JointTrajectory): left arm joint trajectory from
left move group after calling compute_cartesian_path
right_arm (JointTrajectory): right arm joint trajectory from
right move group after calling compute_cartesian_path
tip_poses (list): list of desired end effector poses for
both arms for a particular segment of a primitive plan
Returns:
dict: Dictionary of combined trajectories in different formats.
Keys for each arm, 'right' and 'left', which each are
themselves dictionary. Deeper keys ---
'fk': Cartesian path numpy array, unaligned
'joints': C-space path numpy array, unaligned
'aligned_fk': Cartesian path numpy array, aligned
'aligned_joints': C-space path numpy array, aligned
'closest_inds': Indices of each original path corresponding
to the closest value in the other arms trajectory
"""
# find the longer trajectory
long_traj = 'left' if len(left_arm.points) > len(right_arm.points) \
else 'right'
# make numpy array of each arm joint trajectory for each comp
left_arm_joints_np = np.zeros((len(left_arm.points), 7))
right_arm_joints_np = np.zeros((len(right_arm.points), 7))
# make numpy array of each arm pose trajectory, based on fk
left_arm_fk_np = np.zeros((len(left_arm.points), 7))
right_arm_fk_np = np.zeros((len(right_arm.points), 7))
for i, point in enumerate(left_arm.points):
left_arm_joints_np[i, :] = point.positions
pose = self.compute_fk(point.positions, arm='left')
left_arm_fk_np[i, :] = util.pose_stamped2list(pose)
for i, point in enumerate(right_arm.points):
right_arm_joints_np[i, :] = point.positions
pose = self.compute_fk(point.positions, arm='right')
right_arm_fk_np[i, :] = util.pose_stamped2list(pose)
closest_left_inds = []
closest_right_inds = []
# for each tip_pose, find the index in the longer trajectory that
# most closely matches the pose (using fk)
for i in range(len(tip_poses)):
r_waypoint = util.pose_stamped2list(tip_poses[i][1])
l_waypoint = util.pose_stamped2list(tip_poses[i][0])
r_pos_diffs = util.pose_difference_np(
pose=right_arm_fk_np, pose_ref=np.array(r_waypoint))[0]
l_pos_diffs = util.pose_difference_np(
pose=left_arm_fk_np, pose_ref=np.array(l_waypoint))[0]
# r_pos_diffs, r_ori_diffs = util.pose_difference_np(
# pose=right_arm_fk_np, pose_ref=np.array(r_waypoint))
# l_pos_diffs, l_ori_diffs = util.pose_difference_np(
# pose=left_arm_fk_np, pose_ref=np.array(l_waypoint))
r_index = np.argmin(r_pos_diffs)
l_index = np.argmin(l_pos_diffs)
# r_index = np.argmin(r_pos_diffs + r_ori_diffs)
# l_index = np.argmin(l_pos_diffs + l_ori_diffs)
closest_right_inds.append(r_index)
closest_left_inds.append(l_index)
# Create a new trajectory for the shorter trajectory, that is the same
# length as the longer trajectory.
if long_traj == 'left':
new_right = np.zeros((left_arm_joints_np.shape))
prev_r_ind = 0
prev_new_ind = 0
for i, r_ind in enumerate(closest_right_inds):
# Put the joint values from the short
# trajectory at the indices corresponding to the path waypoints
# at the corresponding indices found for the longer trajectory
new_ind = closest_left_inds[i]
new_right[new_ind, :] = right_arm_joints_np[r_ind, :]
# For the missing values in between the joint waypoints,
# interpolate to fill the trajectory
# if new_ind - prev_new_ind > -1:
interp = np.linspace(right_arm_joints_np[prev_r_ind, :],
right_arm_joints_np[r_ind],
num=new_ind - prev_new_ind)
new_right[prev_new_ind:new_ind, :] = interp
prev_r_ind = r_ind
prev_new_ind = new_ind
aligned_right_joints = new_right
aligned_left_joints = left_arm_joints_np
else:
new_left = np.zeros((right_arm_joints_np.shape))
prev_l_ind = 0
prev_new_ind = 0
for i, l_ind in enumerate(closest_left_inds):
new_ind = closest_right_inds[i]
new_left[new_ind, :] = left_arm_joints_np[l_ind, :]
interp = np.linspace(left_arm_joints_np[prev_l_ind, :],
left_arm_joints_np[l_ind],
num=new_ind - prev_new_ind)
new_left[prev_new_ind:new_ind, :] = interp
prev_l_ind = l_ind
prev_new_ind = new_ind
aligned_right_joints = right_arm_joints_np
aligned_left_joints = new_left
# get aligned poses of the end effector as well
aligned_right_fk = np.zeros((aligned_right_joints.shape[0], 7))
aligned_left_fk = np.zeros((aligned_left_joints.shape[0], 7))
for i in range(aligned_right_joints.shape[0]):
pose_r = self.compute_fk(aligned_right_joints[i, :], arm='right')
aligned_right_fk[i, :] = util.pose_stamped2list(pose_r)
pose_l = self.compute_fk(aligned_left_joints[i, :], arm='left')
aligned_left_fk[i, :] = util.pose_stamped2list(pose_l)
unified = {}
unified['right'] = {}
unified['right']['fk'] = right_arm_fk_np
unified['right']['joints'] = right_arm_joints_np
unified['right']['aligned_fk'] = aligned_right_fk
unified['right']['aligned_joints'] = aligned_right_joints
unified['right']['inds'] = closest_right_inds
unified['left'] = {}
unified['left']['fk'] = left_arm_fk_np
unified['left']['joints'] = left_arm_joints_np
unified['left']['aligned_fk'] = aligned_left_fk
unified['left']['aligned_joints'] = aligned_left_joints
unified['left']['inds'] = closest_left_inds
return unified
def tip_to_wrist(self, tip_poses):
"""
Transform a pose from the Yumi Gelslim tip to the
wrist joint
Args:
tip_poses (dict): Dictionary of PoseStamped values
corresponding to each arm, keyed by 'right' and
'left'
Returns:
dict: Keyed by 'right' and 'left', values are PoseStamped
"""
tip_to_wrist = util.list2pose_stamped(self.cfg.TIP_TO_WRIST_TF, '')
world_to_world = util.unit_pose()
wrist_left = util.convert_reference_frame(tip_to_wrist, world_to_world,
tip_poses['left'],
"yumi_body")
wrist_right = util.convert_reference_frame(tip_to_wrist,
world_to_world,
tip_poses['right'],
"yumi_body")
wrist_poses = {}
wrist_poses['right'] = wrist_right
wrist_poses['left'] = wrist_left
return wrist_poses
def wrist_to_tip(self, wrist_poses):
"""
Transform a pose from the Yumi wrist joint to the
Gelslim tip to the
Args:
wrist_poses (dict): Dictionary of PoseStamped values
corresponding to each arm, keyed by 'right' and
'left'
Returns:
dict: Keyed by 'right' and 'left', values are PoseStamped
"""
wrist_to_tip = util.list2pose_stamped(self.cfg.WRIST_TO_TIP_TF, '')
tip_left = util.convert_reference_frame(wrist_to_tip, util.unit_pose(),
wrist_poses['left'],
"yumi_body")
tip_right = util.convert_reference_frame(wrist_to_tip,
util.unit_pose(),
wrist_poses['right'],
"yumi_body")
tip_poses = {}
tip_poses['right'] = tip_right
tip_poses['left'] = tip_left
return tip_poses
def is_in_contact(self, object_id):
"""
Checks whether or not robot is in contact with a
particular object
Args:
object_id (int): Pybullet object ID of object contact
is checked with
Returns:
dict: Keyed by 'right' and 'left', values are bools.
True means arm 'right/left' is in contact, else False
"""
# r_pts = p.getContactPoints(
# bodyA=self.yumi_pb.arm.robot_id, bodyB=object_id, linkIndexA=12, physicsClientId=pb_util.PB_CLIENT)
# l_pts = p.getContactPoints(
# bodyA=self.yumi_pb.arm.robot_id, bodyB=object_id, linkIndexA=25, physicsClientId=pb_util.PB_CLIENT)
r_pts = p.getContactPoints(
bodyA=self.yumi_pb.arm.robot_id,
bodyB=object_id,
linkIndexA=self.cfg.RIGHT_GEL_ID,
physicsClientId=self.yumi_pb.pb_client.get_client_id())
l_pts = p.getContactPoints(
bodyA=self.yumi_pb.arm.robot_id,
bodyB=object_id,
linkIndexA=self.cfg.LEFT_GEL_ID,
physicsClientId=self.yumi_pb.pb_client.get_client_id())
r_contact_bool = 0 if len(r_pts) == 0 else 1
l_contact_bool = 0 if len(l_pts) == 0 else 1
contact_bool = {}
contact_bool['right'] = r_contact_bool
contact_bool['left'] = l_contact_bool
return contact_bool
def get_jpos(self, arm=None):
"""
Getter function for getting the robot's joint positions
Args:
arm (str, optional): Which arm to get the position for.
Defaults to None, if None will return position of
both arms.
Returns:
list: Joint positions, len() = DOF of single arm
"""
if arm is None:
jpos = self.yumi_pb.arm.get_jpos()
elif arm == 'left':
jpos = self.yumi_pb.arm.arms['left'].get_jpos()
elif arm == 'right':
jpos = self.yumi_pb.arm.arms['right'].get_jpos()
else:
raise ValueError('Arm not recognized')
return jpos
def get_ee_pose(self, arm='right'):
"""
Getter function for getting the robot end effector pose
Args:
arm (str, optional): Which arm to get the EE pose for.
Defaults to 'right'.
Returns:
4-element tuple containing
- np.ndarray: x, y, z position of the EE (shape: :math:`[3,]`)
- np.ndarray: quaternion representation of the
EE orientation (shape: :math:`[4,]`)
- np.ndarray: rotation matrix representation of the
EE orientation (shape: :math:`[3, 3]`)
- np.ndarray: euler angle representation of the
EE orientation (roll, pitch, yaw with
static reference frame) (shape: :math:`[3,]`)
"""
if arm == 'right':
ee_pose = self.yumi_pb.arm.get_ee_pose(arm='right')
elif arm == 'left':
ee_pose = self.yumi_pb.arm.get_ee_pose(arm='left')
else:
raise ValueError('Arm not recognized')
return ee_pose
def get_palm_y_normals(self, palm_poses=None, *args):
"""
Gets the updated world frame normal direction of the palms
"""
normal_y = util.list2pose_stamped([0, 1, 0, 0, 0, 0, 1])
normal_y_poses_world = {}
wrist_poses = {}
for arm in ['right', 'left']:
if palm_poses is None:
wrist_pos_world = self.get_ee_pose(arm=arm)[0].tolist()
wrist_ori_world = self.get_ee_pose(arm=arm)[1].tolist()
wrist_poses[arm] = util.list2pose_stamped(wrist_pos_world +
wrist_ori_world)
else:
wrist_poses[arm] = palm_poses[arm]
tip_poses = self.wrist_to_tip(wrist_poses)
normal_y_poses_world['right'] = util.transform_pose(
normal_y, tip_poses['right'])
normal_y_poses_world['left'] = util.transform_pose(
normal_y, tip_poses['left'])
return normal_y_poses_world
def get_palm_z_normals(self, palm_poses=None, *args):
"""
Gets the updated world frame normal direction of the palms
"""
normal_z = util.list2pose_stamped([0, 0, 1, 0, 0, 0, 1])
normal_z_poses_world = {}
wrist_poses = {}
for arm in ['right', 'left']:
if palm_poses is None:
wrist_pos_world = self.get_ee_pose(arm=arm)[0].tolist()
wrist_ori_world = self.get_ee_pose(arm=arm)[1].tolist()
wrist_poses[arm] = util.list2pose_stamped(wrist_pos_world +
wrist_ori_world)
else:
wrist_poses[arm] = palm_poses[arm]
tip_poses = self.wrist_to_tip(wrist_poses)
normal_z_poses_world['right'] = util.transform_pose(
normal_z, tip_poses['right'])
normal_z_poses_world['left'] = util.transform_pose(
normal_z, tip_poses['left'])
return normal_z_poses_world
def get_current_tip_poses(self):
wrist_poses = {}
for arm in ['right', 'left']:
wrist_pos_world = self.get_ee_pose(arm=arm)[0].tolist()
wrist_ori_world = self.get_ee_pose(arm=arm)[1].tolist()
wrist_poses[arm] = util.list2pose_stamped(wrist_pos_world +
wrist_ori_world)
tip_poses = self.wrist_to_tip(wrist_poses)
return tip_poses
def build_tree(iter, treeA, treeB, edgesA, edgesB, plot, kdl_kin):
i = 0
while i < iter:
# print(i)
jointsA = np.random.rand(1, 7)[0] * [2.5, 2.5, 4, 1.5, 4, 2.5, 4] - [
1.25, 1.25, 2.0, .75, 2.0, .75, 2.0
]
velA = np.random.rand(1, 7)[0] * [3.0, 3.0, 3.0, 3.0, 6.0, 3.0, 6.0
] - [1.5, 1.5, 1.5, 1.5, 2, 1.5, 2]
node = nearest_neighbor(jointsA, velA, treeA, i)
treeA = insert_vertex(node, treeA, jointsA, velA, i, 1)
edgesA = insert_edge(edgesA, node, i)
jointsB = np.random.rand(1, 7)[0] * [2.5, 2.5, 4, 1.5, 4, 2.5, 4] - [
1.25, 1.25, 2.0, .75, 2.0, .75, 2.0
]
velB = np.random.rand(1, 7)[0] * [3.0, 3.0, 3.0, 3.0, 6.0, 3.0, 6.0
] - [1.5, 1.5, 1.5, 1.5, 2, 1.5, 2]
node = nearest_neighbor(jointsB, velB, treeB, i)
treeB = insert_vertex(node, treeB, jointsB, velB, i, 1)
edgesB = insert_edge(edgesB, node, i)
result = connect_trees(treeA, treeB, i, kdl_kin)
# print(result[0],i)
if (result[0]):
print "iterated up to: "
print i
break
i = i + 1
iterations = i
if (plot):
# plt.close('all')
# fig = plt.figure()
# ax = fig.gca(projection='3d')
# ax.scatter(treeA[0,0],treeA[0,1],treeA[0,2],'r')
# ax.plot(treeA[0:i+2,0],treeA[0:i+2,1],treeA[0:i+2,2],'r.')
# ax.scatter(treeB[0,0],treeB[0,1],treeB[0,2],'b')
# ax.plot(treeB[0:i+2,0],treeB[0:i+2,1],treeB[0:i+2,2],'b.')
# plt.show(block=False)
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
pos3A = np.empty((iterations, 3))
for i in range(iterations):
pos3A[i, :] = kdl_kin.forward(treeA[i, :7])[0:3, 3].transpose()
pos3B = np.empty((iterations, 3))
for i in range(iterations):
pos3B[i, :] = kdl_kin.forward(treeB[i, :7])[0:3, 3].transpose()
fig = plt.figure(1)
plt.hold(False)
# ax = fig.gca(projection='3d')
plt.plot(pos3A[:, 1], pos3A[:, 2], 'r.')
plt.hold(True)
plt.plot(pos3A[0, 1], pos3A[0, 2], 'r.', markersize=15)
plt.plot(pos3B[:, 1], pos3B[:, 2], 'b.')
plt.plot(pos3B[0, 1], pos3B[0, 2], 'b.', markersize=15)
plt.xlabel('EE y-coordinate')
plt.ylabel('EE z-coordinate')
plt.title('RRT in end-effector space')
# plt.show(block=False)
if (result[0]):
indA = result[1]
indB = result[2]
# if (plot):
# ax.scatter([treeA[indA,0], treeB[indB,0]],[treeA[indA,1], treeB[indB,1]],[treeA[indA,2], treeB[indB,2]],'g')
else:
indA = 0
indB = 0
if (plot):
# plt.xlim([-1.5, 1.5])
# plt.ylim([-1.5, 1.5])
for k in range(iterations + 1):
edge1 = kdl_kin.forward(treeA[edgesA[k, 0], :7])[0:3,
3].transpose()
edge2 = kdl_kin.forward(treeA[edgesA[k, 1], :7])[0:3,
3].transpose()
plt.plot([edge1[0, 1], edge2[0, 1]], [edge1[0, 2], edge2[0, 2]],
'r',
linewidth=3)
edge1 = kdl_kin.forward(treeB[edgesB[k, 0], :7])[0:3,
3].transpose()
edge2 = kdl_kin.forward(treeB[edgesB[k, 1], :7])[0:3,
3].transpose()
plt.plot([edge1[0, 1], edge2[0, 1]], [edge1[0, 2], edge2[0, 2]],
'b',
linewidth=3)
# plt.show(block=False)
# raw_input('Enter...')
return treeA[0:iterations +
2, :], treeB[0:iterations +
2, :], edgesA[0:iterations +
1, :], edgesB[0:iterations +
1, :], indA, indB
def execute_move(self, pos):
rospy.loginfo('moving')
# Read in pose data
q = [pos.orientation.w, pos.orientation.x, pos.orientation.y, pos.orientation.z]
p =[[pos.position.x],[pos.position.y],[pos.position.z]]
# Convert quaternion data to rotation matrix
R = quat_to_so3(q);
# Form transformation matrix
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
# Create seed with current position
q0 = kdl_kin.random_joint_angles()
limb_interface = baxter_interface.limb.Limb('right')
current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
for ind in range(len(q0)):
q0[ind] = current_angles[ind]
pose = kdl_kin.forward(q0)
Xstart = copy(np.asarray(pose))
pose[0:3,0:3] = R
pose[0:3,3] = p
Xend = copy(np.asarray(pose))
# Compute straight-line trajectory for path
N = 500
Xlist = CartesianTrajectory(Xstart, Xend, 1, N, 5)
thList = np.empty((N,7))
thList[0] = q0;
for i in range(N-1):
# Solve for joint angles
seed = 0
q_ik = kdl_kin.inverse(Xlist[i+1], thList[i])
while q_ik == None:
seed += 0.3
q_ik = kdl_kin.inverse(pose, q0+seed)
thList[i+1] = q_ik
# rospy.loginfo(q_ik)
# # Solve for joint angles
# seed = 0.3
# q_ik = kdl_kin.inverse(pose, q0+seed)
# while q_ik == None:
# seed += 0.3
# q_ik = kdl_kin.inverse(pose, q0+seed)
# rospy.loginfo(q_ik)
# q_list = JointTrajectory(q0,q_ik,1,100,5)
# for q in q_list:
# Format joint angles as limb joint angle assignment
angles = limb_interface.joint_angles()
angle_count = 0
for ind, joint in enumerate(limb_interface.joint_names()):
# if fabs(angles[joint] - q_ik[ind]) < .05:
# angle_count += 1
angles[joint] = q_ik[ind]
# rospy.loginfo(angles)
rospy.sleep(.003)
# Send joint move command
rospy.loginfo('move to object')
# rospy.loginfo(angle_count)
# if angle_count > 4:
# nothing = True;
# else:
limb_interface.set_joint_position_speed(.3)
limb_interface.set_joint_positions(angles)
self._done = True
print('Done')
def get_jaco_kinematics(self):
kin = KDLKinematics(self.urdf, self.base_link, self.end_link)
return kin
else:
print "The object detection result is not valid!"
object_found = False
# the names of the base_link and end_link according to the urdf model
base_link = "iiwa_0_link"
end_link = "iiwa_adapter"
# initialize a node
rospy.init_node("manipulator_mover")
robot = URDF.from_xml_file(
"/home/davoud/kuka_ws/src/kuka/kuka/iiwa/iiwa_description/urdf/iiwa.urdf")
tree = kdl_tree_from_urdf_model(robot)
kdl_kin = KDLKinematics(robot, base_link, end_link)
# initialize a publisher for publishing the needed data for object pose saving in a file
# file_data_pub = rospy.Publisher("/vrep_ros_communication/file_data", FileStorageInf, queue_size=1)
# actionlib commands
manipulator_client = actionlib.SimpleActionClient(
"/iiwa/follow_joint_trajectory", FollowJointTrajectoryAction)
manipulator_client.wait_for_server()
trajectory = JointTrajectory()
trajectory.joint_names = joint_names
trajectory.points.append(JointTrajectoryPoint())
# subscribe to "/joint_states" topic which is published by kuka.
rospy.Subscriber("/joint_states", JointState, kuka_inf_callback)
def __init__(self):
limb = read_parameter('~limb', 'right')
if limb not in ['right', 'left']:
rospy.logerr('Unknown limb name [%s]' % limb)
return
# Read the controllers parameters
gains = read_parameter('~gains', dict())
self.kp = joint_list_to_kdl(gains['Kp'])
self.kd = joint_list_to_kdl(gains['Kd'])
self.publish_rate = read_parameter('~publish_rate', 500)
self.frame_id = read_parameter('~frame_id', 'base')
self.timeout = read_parameter('~timeout', 0.02) # seconds
# Set-up baxter interface
self.arm_interface = baxter_interface.Limb(limb)
# set_joint_torques must be commanded at a rate great than the timeout specified by set_command_timeout.
# If the timeout is exceeded before a new set_joint_velocities command is received, the controller will switch
# modes back to position mode for safety purposes.
self.arm_interface.set_command_timeout(self.timeout)
# Baxter kinematics
self.urdf = URDF.from_parameter_server(key='robot_description')
self.tip_link = '%s_gripper' % limb
self.kinematics = KDLKinematics(self.urdf, self.frame_id,
self.tip_link)
self.fk_vel_solver = PyKDL.ChainFkSolverVel_recursive(
self.kinematics.chain)
# Adjust the publish rate of baxter's state
joint_state_pub_rate = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
# Get the joint names and limits
self.joint_names = self.arm_interface.joint_names()
self.num_joints = len(self.joint_names)
self.lower_limits = np.zeros(self.num_joints)
self.upper_limits = np.zeros(self.num_joints)
# KDL joint may be in a different order than expected
kdl_lower_limits, kdl_upper_limits = self.kinematics.get_joint_limits()
for i, name in enumerate(self.kinematics.get_joint_names()):
if name in self.joint_names:
idx = self.joint_names.index(name)
self.lower_limits[idx] = kdl_lower_limits[i]
self.upper_limits[idx] = kdl_upper_limits[i]
self.middle_values = (self.upper_limits + self.lower_limits) / 2.0
# Move the arm to a neutral position
#~ self.arm_interface.move_to_neutral(timeout=10.0)
#~ neutral_pos = [-math.pi/4, 0.0, 0.0, 0.0, 0.0, math.pi/2, -math.pi/2]
neutral_pos = [1.0, -0.57, -1.2, 1.3, 0.86, 1.4, -0.776]
#~ neutral_pos = [pi/4, -pi/3, 0.0, pi/2, 0.0, pi/3, 0.0]
neutral_cmd = dict()
for i, name in enumerate(self.joint_names):
if name in ['left_s0', 'left_w2']:
neutral_cmd[name] = neutral_pos[i] * -1.0
else:
neutral_cmd[name] = neutral_pos[i]
self.arm_interface.move_to_joint_positions(neutral_cmd, timeout=10.0)
# Baxter faces
self.limb = limb
self.face = FaceImage()
self.face.set_image('happy')
# Set-up publishers/subscribers
self.suppress_grav_comp = rospy.Publisher(
'/robot/limb/%s/suppress_gravity_compensation' % limb, Empty)
joint_state_pub_rate.publish(int(self.publish_rate))
self.feedback_pub = rospy.Publisher('/takktile/force_feedback', Wrench)
rospy.Subscriber('/baxter/%s_ik_command' % limb, PoseStamped,
self.ik_command_cb)
self.gripper_closed = False
rospy.Subscriber('/takktile/calibrated', Touch, self.takktile_cb)
rospy.Subscriber('/robot/end_effector/%s_gripper/state' % limb,
EndEffectorState, self.end_effector_cb)
rospy.loginfo('Running Cartesian controller for the %s arm' % limb)
# Start torque controller timer
self.cart_command = None
self.torque_controller_timer = rospy.Timer(
rospy.Duration(1.0 / self.publish_rate), self.torque_controller_cb)
# Shutdown hookup to clean-up before killing the script
rospy.on_shutdown(self.shutdown)
class Robot(object):
def __init__(self, sim=True):
f = file('/home/hirolab/catkin_ws/src/vspgrid/urdf/widowx2.urdf', 'r')
robot = URDF.from_xml_string(f.read()) # parsed URDF
# robot = URDF.from_parameter_server()
self.kin = KDLKinematics(robot, 'base', 'mconn')
# Selection of end-effector parameters for WidowX
# x, y, z, yaw, pitch
self.cart_from_matrix = lambda T: np.array([
T[0, 3],
T[1, 3],
T[2, 3],
math.atan2(T[1, 0], T[0, 0]),
-math.asin(T[2, 0])])
self.home = np.array([0.05, 0, 0.25, 0, 0]) # home end-effector pose
self.q = np.array([0, 0, 0, 0, 0]) # joint angles
self.dt = 0.05 # algorithm time step
self.sim = sim # true if virtual robot
def publish(eventStop):
# for arbotix
pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)
# for visualization
jointPub = rospy.Publisher(
"/joint_states", JointState, queue_size=5)
jmsg = JointState()
jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']
while not rospy.is_shutdown() and not eventStop.is_set():
jmsg.header.stamp = rospy.Time.now()
jmsg.position = self.q
if self.sim:
jointPub.publish(jmsg)
pub1.publish(self.q[0])
pub2.publish(self.q[1])
pub3.publish(self.q[2])
pub4.publish(self.q[3])
pub5.publish(self.q[4])
eventStop.wait(self.dt)
rospy.init_node("robot")
eventStop = threading.Event()
threadJPub = threading.Thread(target=publish, args=(eventStop,))
threadJPub.daemon = True
threadJPub.start() # Update joint angles in a background process
def act_gripper(self, grip):
# Actuate gripper given gripper position
pub = rospy.Publisher('q6/command', Float64, queue_size=5)
rospy.sleep(1)
pub.publish(grip)
rospy.sleep(1)
def forward(self, q=None):
# End-effector transformation matrix, given joint angles
if q is None:
q = self.q
return(self.kin.forward(q))
def cart(self, q=None):
# Cartesian coordinates [x, y, z, r1, r2, r3], given joint angles
if q is None:
q = self.q
return(self.cart_from_matrix(self.forward(q)))
def jacobian(self, q=None):
# Inertial jacobian
if q is None:
q = self.q
eps = 1e-3
s0 = np.array(self.cart(q))
dof = len(self.q)
J = np.array(dof * [dof * [0.0]])
for k in range(0, dof):
dq = np.zeros(dof)
dq[k] = eps
J[:, k] = (self.cart(q + dq) - s0) / eps
return J
def move(self, s1, speed=1):
def plan(s1):
# return tuples of joint coordinates to be moved at every time step
# Error function to find the zero
def error_function(q): return (
np.array(self.cart(q)) - np.array(s1))
x, _, ler, message = scipy.optimize.fsolve(
error_function, 0*self.q, full_output=True)
if not ler == 1:
print('No IK solution found')
q1 = self.q
else:
q1 = (x + math.pi) % (2 * math.pi) - math.pi
# q1 = x
# break trajectory to intermediate points
q0 = self.q # initial joint angles
s0 = self.cart(q0) # initial end-effector pose
t1 = np.max(np.abs(q1 - q0)) / speed # time to reach goal
time = np.arange(0, t1, self.dt) + self.dt # time array
dof = len(self.q)
# Trajectory matrix
qq = np.array([np.interp(time, [0, t1], [q0[k], q1[k]])
for k in range(dof)]).transpose()
return qq
traj = plan(s1)
rate = rospy.Rate(1.0 / self.dt)
for joints in traj:
self.q = joints
rate.sleep()
def move2(self, s1):
# Not used
speed = 1.5
TOL = 1e-8
MAX_VEL = 0.2
t0 = rospy.Time.now()
while rospy.Time.now() - t0 < rospy.Duration(5):
ds = s1 - self.cart()
dev = np.linalg.norm(ds)
if dev < TOL:
print("Success. Deviation: ")
print(dev)
return
J = self.jacobian()
dq = np.linalg.solve(J, ds)
if max(abs(dq)) > MAX_VEL:
dq = dq * MAX_VEL / max(abs(dq))
alfa = speed * self.dt * (rospy.Time.now() - t0).to_sec()**1.5
self.q = self.q + dq * alfa
rospy.sleep(self.dt)
print("Failure. Deviation: ")
print(dev)
def reset(self):
self.move(self.cart(self.q*0))
class Mission:
def __init__(self, name):
self.name = name
self.timer = 0
# the names of the base_link and end_link according to the urdf model
self.base_link = "iiwa_0_link"
self.end_link = "iiwa_adapter"
# initialize a node
rospy.init_node("mission_node")
self.robot = URDF.from_xml_file(
"/home/davoud/kuka_ws/src/kuka/kuka/iiwa/iiwa_description/urdf/iiwa.urdf"
)
self.tree = kdl_tree_from_urdf_model(self.robot)
self.kdl_kin = KDLKinematics(self.robot, self.base_link, self.end_link)
# you can use one of these methods:
# 1: Jacobian Transpose Method (JTM)
# 2: Jacobian Pseudoinverse Method (JPM)
# 3: Damped Least Squares Method (DLSM)
self.used_method = "DLSM"
self.landa = 0.1
# tf broadcaster
self.br = tf.TransformBroadcaster()
# current position of the end effector
self.end_eff_current_pose = np.empty((1, 7))
# transformation matrix between end_effector and camera
self.end_cam_T = np.empty((4, 4))
self.set_end_cam_T()
# transformation matrix between end_effector and gripper
self.end_grip_T = np.empty((4, 4))
self.set_end_grip_T()
# transformation matrix between gripper and target pose
self.grip_target_T = np.empty((4, 4))
self.set_grip_target_T()
# transformation matrix between base and end effector
self.base_end_T = np.empty((4, 4))
# transformation matrix between base and target
self.base_target_T = np.empty((4, 4))
# needed variables for actionlib
self.joint_names = [
"joint1", "joint2", "joint3", "joint4", "joint5", "joint6",
"joint7"
]
# actionlib commands
self.manipulator_client = actionlib.SimpleActionClient(
"/iiwa/follow_joint_trajectory", FollowJointTrajectoryAction)
self.manipulator_client.wait_for_server()
self.trajectory = JointTrajectory()
self.trajectory.joint_names = self.joint_names
self.trajectory.points.append(JointTrajectoryPoint())
# path of end_effector
self.ee_path = MarkerArray()
# path of object
self.object_path = MarkerArray()
# initializ a publisher to publish the marker of detected objet to rviz
self.marker_object_in_base_pub = rospy.Publisher(
"marker_object_in_base", Marker, queue_size=1)
self.marker_object_in_cam_pub = rospy.Publisher("marker_object_in_cam",
Marker,
queue_size=1)
self.marker_eff_path_pub = rospy.Publisher("marker_eff_path",
MarkerArray,
queue_size=1)
self.marker_obj_path_pub = rospy.Publisher("marker_obj_path",
MarkerArray,
queue_size=1)
# initialize the service client for the gripper
rospy.wait_for_service("/sdh_action")
self.joint_config_client = rospy.ServiceProxy("/sdh_action", SDHAction)
# subscribe to "/joint_states" topic which is published by kuka.
rospy.Subscriber("/joint_states", JointState, self.kuka_inf_cb)
# subscribe to "/sphere_coefs" topic which is published by ball detector using color and distance filters
rospy.Subscriber("/sphere_coefs",
Float32MultiArray,
self.marker_inf_cb,
queue_size=1)
rospy.spin()
def kuka_inf_cb(self, data):
if data._connection_header["callerid"] != "/kuka_manager":
return
self.end_eff_current_pose = data.position
self.set_base_end_T()
def marker_inf_cb(self, data):
# in order to filter the inamissible (0.0, 0.0, 0.0, -inf) detections
if data.data[0:3] == (0.0, 0.0, 0.0):
# print "inadmissible input!"
return
vec3 = Vector3()
vec3.x = data.data[0]
vec3.y = data.data[1]
vec3.z = data.data[2]
ball_pose = np.array([vec3.x, vec3.y, vec3.z])
for i in range(3):
if np.isnan(ball_pose[i]):
return
cam_object_T = self.set_cam_object_T(ball_pose)
cam_object_R = tr.identity_matrix()
cam_object_R[0:3, 0:3] = cam_object_T[0:3, 0:3]
cam_object_q = tr.quaternion_from_matrix(cam_object_R)
self.br.sendTransform(
(cam_object_T[0, 3], cam_object_T[1, 3], cam_object_T[2, 3]),
cam_object_q, rospy.Time.now(), "object_in_cam_frame",
"camera_rgb_optical_frame")
# publishing the object marker to rviz for object_in_cam
det_marker = Marker()
det_marker.header.frame_id = "camera_rgb_optical_frame"
det_marker.header.stamp = rospy.Time.now()
det_marker.type = Marker.SPHERE
det_marker.pose.position.x = cam_object_T[0, 3]
det_marker.pose.position.y = cam_object_T[1, 3]
det_marker.pose.position.z = cam_object_T[2, 3]
det_marker.pose.orientation.x = 0
det_marker.pose.orientation.y = 0
det_marker.pose.orientation.z = 0
det_marker.pose.orientation.w = 1
det_marker.scale.x = 0.05
det_marker.scale.y = 0.05
det_marker.scale.z = 0.05
det_marker.color.a = 1.0
det_marker.color.r = 1.0
det_marker.color.g = 0.0
det_marker.color.b = 0.0
self.marker_object_in_cam_pub.publish(det_marker)
base_object_T = np.dot(np.dot(self.base_end_T, self.end_cam_T),
cam_object_T)
# in order to assign object the same orientation as target frame
base_object_T[0:3, 0:3] = self.base_target_T[0:3, 0:3]
final_err = self.error_computation(base_object_T, self.base_target_T)
# target thresholds:
trans_th = [0.05, 0.05, 0.05]
rot_th = [15 * np.pi / 180, 15 * np.pi / 180, 15 * np.pi / 180]
print "timer: ", self.timer
if self.timer > 5:
print "goal reached ............................"
self.gripper_final_action()
if np.abs(final_err[0]) < trans_th[0] and np.abs(
final_err[1]) < trans_th[1] and np.abs(
final_err[2]) < trans_th[2]:
if np.abs(final_err[3]) < rot_th[0] and np.abs(
final_err[4]) < rot_th[1] and np.abs(
final_err[5]) < rot_th[2]:
self.timer += 1
return
else:
self.timer = 0
print "error :", final_err
else:
self.timer = 0
print "error :", final_err
# use of error_gains:
error_gains = np.reshape(np.array([1, 1, 1, 1, 1, 1]), (6, 1))
final_err2 = np.multiply(final_err, error_gains)
# error value type 2
end_object_T = np.dot(self.end_cam_T, cam_object_T)
end_target_T = np.dot(self.end_grip_T, self.grip_target_T)
object_target_T = np.dot(end_object_T, np.linalg.inv(end_target_T))
# print "error style 2: ", object_target_T
# publishing the object marker to rviz
det_marker = Marker()
det_marker.header.frame_id = "iiwa_base_link"
det_marker.header.stamp = rospy.Time.now()
det_marker.type = Marker.SPHERE
det_marker.pose.position.x = base_object_T[0, 3]
det_marker.pose.position.y = base_object_T[1, 3]
det_marker.pose.position.z = base_object_T[2, 3]
det_marker.pose.orientation.x = 0
det_marker.pose.orientation.y = 0
det_marker.pose.orientation.z = 0
det_marker.pose.orientation.w = 1
det_marker.scale.x = 0.05
det_marker.scale.y = 0.05
det_marker.scale.z = 0.05
det_marker.color.a = 1.0
det_marker.color.r = 0.0
det_marker.color.g = 0.0
det_marker.color.b = 1.0
self.marker_object_in_base_pub.publish(det_marker)
# print "final error is : ", final_err
# print "base_target_T: ", base_object_T
# computation of jacobian of the manipulator
J = self.kdl_kin.jacobian(self.end_eff_current_pose)
if self.used_method == "DLSM":
# use of damped_least_squares as matrix inverse
J_inverse = np.dot(
J.T,
np.linalg.inv(
np.dot(J, J.T) + ((self.landa**2) * np.identity(6))))
elif self.used_method == "JPM":
# use of matrix pseudo_inverse as matrix inverse
J_inverse = np.linalg.pinv(J)
elif self.used_method == "JTM":
# use of matrix transpose as matrix inverse
J_inverse = np.transpose(J)
final_vel = np.dot(J_inverse, final_err2)
# the dereived final speed command for the manipulator
speed_cmd = list(np.array(final_vel).reshape(-1, ))
# avoid huge joint velocities:
joint_vel_th = [0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7]
if speed_cmd[0] > joint_vel_th[0]:
print "huge velocity for the first joint!"
return
elif speed_cmd[1] > joint_vel_th[1]:
print "huge velocity for the second joint!"
return
elif speed_cmd[2] > joint_vel_th[2]:
print "huge velocity for the third joint!"
return
elif speed_cmd[3] > joint_vel_th[3]:
print "huge velocity for the fourth joint!"
return
elif speed_cmd[4] > joint_vel_th[4]:
print "huge velocity for the fifth joint!"
return
elif speed_cmd[5] > joint_vel_th[5]:
print "huge velocity for the sixth joint!"
return
elif speed_cmd[6] > joint_vel_th[6]:
print "huge velocity for the seventh joint!"
return
# block to handle speed publisher using pose publisher
intergration_time_step = 0.3
pose_change = [speed * intergration_time_step for speed in speed_cmd]
final_pose = [
a + b for a, b in zip(self.end_eff_current_pose, pose_change)
]
# actionlib commands
self.trajectory.points[0].positions = final_pose
self.trajectory.points[0].velocities = [0.0] * len(self.joint_names)
self.trajectory.points[0].accelerations = [0.0] * len(self.joint_names)
self.trajectory.points[0].time_from_start = rospy.Duration(1.0)
# print "pose_change", pose_change
# print "The manipulator is moving..."
goal = FollowJointTrajectoryGoal()
goal.trajectory = self.trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
self.manipulator_client.send_goal(goal)
action_result = self.manipulator_client.wait_for_result()
if action_result is True:
# publishing the path of the object in "red" color
final_object_position = base_object_T[0:3, 3]
final_object_R = tr.identity_matrix()
final_object_R[0:3, 0:3] = base_object_T[0:3, 0:3]
final_object_quaternion = tr.quaternion_from_matrix(final_object_R)
self.object_path.markers.append(Marker())
self.object_path.markers[-1].header.frame_id = "iiwa_base_link"
self.object_path.markers[-1].header.stamp = rospy.Time.now()
self.object_path.markers[-1].id = len(self.object_path.markers)
self.object_path.markers[-1].type = Marker.SPHERE
self.object_path.markers[
-1].pose.position.x = final_object_position[0]
self.object_path.markers[
-1].pose.position.y = final_object_position[1]
self.object_path.markers[
-1].pose.position.z = final_object_position[2]
self.object_path.markers[
-1].pose.orientation.x = final_object_quaternion[0]
self.object_path.markers[
-1].pose.orientation.y = final_object_quaternion[1]
self.object_path.markers[
-1].pose.orientation.z = final_object_quaternion[2]
self.object_path.markers[
-1].pose.orientation.w = final_object_quaternion[3]
self.object_path.markers[-1].scale.x = 0.01
self.object_path.markers[-1].scale.y = 0.01
self.object_path.markers[-1].scale.z = 0.01
self.object_path.markers[-1].color.a = 1.0
self.object_path.markers[-1].color.r = 1.0
self.object_path.markers[-1].color.g = 0.0
self.object_path.markers[-1].color.b = 0.0
self.marker_obj_path_pub.publish(self.object_path)
# publishing path of the end effector in "green" color
final_ee_position = self.base_end_T[0:3, 3]
final_ee_R = tr.identity_matrix()
final_ee_R[0:3, 0:3] = self.base_end_T[0:3, 0:3]
final_ee_quaternion = tr.quaternion_from_matrix(final_ee_R)
self.ee_path.markers.append(Marker())
self.ee_path.markers[-1].header.frame_id = "iiwa_base_link"
self.ee_path.markers[-1].header.stamp = rospy.Time.now()
self.ee_path.markers[-1].id = len(self.ee_path.markers)
self.ee_path.markers[-1].type = Marker.SPHERE
self.ee_path.markers[-1].pose.position.x = final_ee_position[0]
self.ee_path.markers[-1].pose.position.y = final_ee_position[1]
self.ee_path.markers[-1].pose.position.z = final_ee_position[2]
self.ee_path.markers[-1].pose.orientation.x = final_ee_quaternion[
0]
self.ee_path.markers[-1].pose.orientation.y = final_ee_quaternion[
1]
self.ee_path.markers[-1].pose.orientation.z = final_ee_quaternion[
2]
self.ee_path.markers[-1].pose.orientation.w = final_ee_quaternion[
3]
self.ee_path.markers[-1].scale.x = 0.01
self.ee_path.markers[-1].scale.y = 0.01
self.ee_path.markers[-1].scale.z = 0.01
self.ee_path.markers[-1].color.a = 1.0
self.ee_path.markers[-1].color.r = 0.0
self.ee_path.markers[-1].color.g = 1.0
self.ee_path.markers[-1].color.b = 0.0
self.marker_eff_path_pub.publish(self.ee_path)
def set_end_cam_T(self):
# translation and 180 degree rotation around z axis ???
delta_x = 0.02
delta_y = 0.175 # it used to be 0.125
delta_z = 0.03
self.end_cam_T = np.array([[-1, 0, 0, delta_x], [0, -1, 0, delta_y],
[0, 0, 1, delta_z], [0, 0, 0, 1]])
# print "end_cam_T: ", self.end_cam_T
def set_end_grip_T(self):
# translation and ??? degree rotation arouund ? axis ???
delta_x = 0.0
delta_y = 0.0
delta_z = 0.7
self.end_grip_T = np.array([[1, 0, 0, delta_x], [0, 1, 0, delta_y],
[0, 0, 1, delta_z], [0, 0, 0, 1]])
# print "end_grip_T: ", self.end_grip_T
"""
o_eulers = tr.euler_from_matrix(self.end_grip_T, 'rxyz')
self.br.sendTransform((delta_x, delta_y, 0),
tf.transformations.quaternion_from_euler(o_eulers[0], o_eulers[1], o_eulers[2]),
rospy.Time.now(),
"gripper",
"iiwa_adapter")
"""
def set_grip_target_T(self):
# translation and no rotation ???
delta_x = 0.0
delta_y = 0.0
delta_z = 0.05
self.grip_target_T = np.array([[1, 0, 0, delta_x], [0, 1, 0, delta_y],
[0, 0, 1, delta_z], [0, 0, 0, 1]])
# print "grip_target_T: ", self.grip_target_T
def set_base_end_T(self):
pose = self.kdl_kin.forward(self.end_eff_current_pose)
self.base_end_T = pose
# print "base_end_T: ", self.base_end_T
self.base_target_T = np.dot(np.dot(self.base_end_T, self.end_grip_T),
self.grip_target_T)
# print "base_target_T: ", self.base_target_T
def set_cam_object_T(self, ball_pose):
Tr = tr.translation_matrix([ball_pose[0], ball_pose[1], ball_pose[2]])
# the rotation angles are zero because the detected ball does not have any orientation
xaxis, yaxis, zaxis = [1, 0, 0], [0, 1, 0], [0, 0, 1]
Rx = tr.rotation_matrix(0, xaxis)
Ry = tr.rotation_matrix(0, yaxis)
Rz = tr.rotation_matrix(0, zaxis)
R = tr.concatenate_matrices(Rx, Ry, Rz)
T = tr.concatenate_matrices(Tr, R)
return T
def gripper_final_action(self):
self.timer = 0
print "in gripper_final_action... "
service_result = self.joint_config_client(0, 0, 0.0, 1.0)
# print "sevice ", service_result
pose_difference = np.array([[1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 1, -0.22], [0, 0, 0, 1]])
final_pose_T = np.dot(self.base_target_T, pose_difference)
print "final_pose_T : ", final_pose_T
final_q_ik = self.kdl_kin.inverse(
final_pose_T,
np.array(self.end_eff_current_pose) + 0.5)
# print "final_q_ik : ", np.shape(final_q_ik), type(final_q_ik)
if final_q_ik is None:
print "The target could not be reached!"
return
# actionlib commands
self.trajectory.points[0].positions = final_q_ik
self.trajectory.points[0].velocities = [0.0] * len(self.joint_names)
self.trajectory.points[0].accelerations = [0.0] * len(self.joint_names)
self.trajectory.points[0].time_from_start = rospy.Duration(1.0)
goal = FollowJointTrajectoryGoal()
goal.trajectory = self.trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
self.manipulator_client.send_goal(goal)
self.manipulator_client.wait_for_result()
service_result = self.joint_config_client(0, 0, 1.0, 1.0)
def error_computation(self, base_object, base_target):
position_err = base_object[0:3, 3] - base_target[0:3, 3]
# computation of orientation_err using the skew symmetric matrix
# the order for the result quaternion from tr package is x, y, z, w
# no need to change the order in goal_q and ee_q
cos = np.cos
sin = np.sin
base_object_R = tr.identity_matrix()
base_object_R[0:3, 0:3] = base_object[0:3, 0:3]
base_object_quaternion = tr.quaternion_from_matrix(base_object_R)
goal_q = [
base_object_quaternion[0], base_object_quaternion[1],
base_object_quaternion[2], base_object_quaternion[3]
]
### object tf wrt base before modification in orientation
real_base_object_quaternion = tr.quaternion_from_matrix(base_object_R)
object_real_q = [
real_base_object_quaternion[0], real_base_object_quaternion[1],
real_base_object_quaternion[2], real_base_object_quaternion[3]
]
self.br.sendTransform(
(base_object[0, 3], base_object[1, 3], base_object[2, 3]),
object_real_q, rospy.Time.now(), "actual_object_tf",
"iiwa_base_link")
# computation of orientation error using skey_symmetric matrix
base_target_R = tr.identity_matrix()
base_target_R[0:3, 0:3] = base_target[0:3, 0:3]
base_target_quaternion = tr.quaternion_from_matrix(base_target_R)
ee_q = [
base_target_quaternion[0], base_target_quaternion[1],
base_target_quaternion[2], base_target_quaternion[3]
]
skew_mat = np.array([[0.0, -goal_q[2], goal_q[1]],
[goal_q[2], 0.0, -goal_q[0]],
[-goal_q[1], goal_q[0], 0.0]])
orientation_err = np.empty((3, 1))
for i in range(3):
orientation_err[i] = (ee_q[3] * goal_q[i]) - (
goal_q[3] * ee_q[i]) - (skew_mat[i, 0] * ee_q[0] +
skew_mat[i, 1] * ee_q[1] +
skew_mat[i, 2] * ee_q[2])
# print "orientation error: ", orientation_err
final_err = np.empty((6, 1))
final_err[0] = position_err[0]
final_err[1] = position_err[1]
final_err[2] = position_err[2]
# final_err[0] = 0
# final_err[1] = 0
# final_err[2] = 0
final_err[3] = orientation_err[0]
final_err[4] = orientation_err[1]
final_err[5] = orientation_err[2]
# final_err[3] = 0
# final_err[4] = 0
# final_err[5] = 0
return final_err
class Autocamera:
DEBUG = False # Print debug messages?
def __init__(self):
self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
self.psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
self.psm2_kin = KDLKinematics(self.psm2_robot, self.psm2_robot.links[0].name, self.psm2_robot.links[-1].name)
self.zoom_level_positions = {'l1':None, 'r1':None, 'l2':None, 'r2':None, 'lm':None, 'rm':None}
self.logerror("autocamera_initialized")
def logerror(self, msg, debug = False):
if self.DEBUG or debug:
rospy.logerr(msg)
def dotproduct(self, v1, v2):
return sum((a*b) for a, b in zip(v1, v2))
def length(self, v):
return math.sqrt(dotproduct(v, v))
def angle(self, v1, v2):
return math.acos(dotproduct(v1, v2) / (length(v1) * length(v2)))
def column(self, matrix, i):
return [row[i] for row in matrix]
def find_rotation_matrix_between_two_vectors(self, a,b):
a = numpy.array(a).reshape(1,3)[0].tolist()
b = numpy.array(b).reshape(1,3)[0].tolist()
vector_orig = a / norm(a)
vector_fin = b / norm(b)
# The rotation axis (normalised).
axis = cross(vector_orig, vector_fin)
axis_len = norm(axis)
if axis_len != 0.0:
axis = axis / axis_len
# Alias the axis coordinates.
x = axis[0]
y = axis[1]
z = axis[2]
# The rotation angle.
angle = acos(dot(vector_orig, vector_fin))
# Trig functions (only need to do this maths once!).
ca = cos(angle)
sa = sin(angle)
R = numpy.identity(3)
# Calculate the rotation matrix elements.
R[0,0] = 1.0 + (1.0 - ca)*(x**2 - 1.0)
R[0,1] = -z*sa + (1.0 - ca)*x*y
R[0,2] = y*sa + (1.0 - ca)*x*z
R[1,0] = z*sa+(1.0 - ca)*x*y
R[1,1] = 1.0 + (1.0 - ca)*(y**2 - 1.0)
R[1,2] = -x*sa+(1.0 - ca)*y*z
R[2,0] = -y*sa+(1.0 - ca)*x*z
R[2,1] = x*sa+(1.0 - ca)*y*z
R[2,2] = 1.0 + (1.0 - ca)*(z**2 - 1.0)
R = numpy.matrix(R)
return R
def extract_positions(self, joint, arm_name, joint_kin=None):
arm_name = arm_name.lower()
if arm_name=="ecm": joint_kin = self.ecm_kin
elif arm_name =="psm1" : joint_kin = self.psm1_kin
elif arm_name =="psm2" : joint_kin = self.psm2_kin
pos = []
name = []
effort = []
velocity = []
new_joint = JointState()
for i in range(len(joint.position)):
if joint.name[i] in joint_kin.get_joint_names():
pos.append(joint.position[i])
name.append(joint.name[i])
new_joint.name = name
new_joint.position = pos
return new_joint
def add_marker(self, pose, name, color=[1,0,1], type=Marker.SPHERE, scale = [.02,.02,.02], points=None):
vis_pub = rospy.Publisher(name, Marker, queue_size=10)
marker = Marker()
marker.header.frame_id = "world"
marker.header.stamp = rospy.Time()
marker.ns = "my_namespace"
marker.id = 0
marker.type = type
marker.action = Marker.ADD
if type == Marker.LINE_LIST:
for point in points:
p = Point()
p.x = point[0]
p.y = point[1]
p.z = point[2]
marker.points.append(p)
else:
r = self.find_rotation_matrix_between_two_vectors([1,0,0], [0,0,1])
rot = pose[0:3,0:3] * r
pose2 = numpy.matrix(numpy.identity(4))
pose2[0:3,0:3] = rot
pose2[0:3,3] = pose[0:3,3]
quat_pose = PoseConv.to_pos_quat(pose2)
marker.pose.position.x = quat_pose[0][0]
marker.pose.position.y = quat_pose[0][1]
marker.pose.position.z = quat_pose[0][2]
marker.pose.orientation.x = quat_pose[1][0]
marker.pose.orientation.y = quat_pose[1][1]
marker.pose.orientation.z = quat_pose[1][2]
marker.pose.orientation.w = quat_pose[1][3]
marker.scale.x = scale[0]
marker.scale.y = scale[1]
marker.scale.z = scale[2]
marker.color.a = .5
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
vis_pub.publish(marker)
def point_towards_midpoint(self, clean_joints, psm1_pos, psm2_pos, key_hole,ecm_pose):
mid_point = (psm1_pos + psm2_pos)/2
# mid_point = ecm_pose[0:3,3] - numpy.array([0,0,.01]).reshape(3,1)
self.add_marker(PoseConv.to_homo_mat([mid_point, [0,0,0]]), '/marker_subscriber',color=[1,0,0], scale=[0.047/5,0.047/5,0.047/5])
self.add_marker(PoseConv.to_homo_mat([key_hole,[0,0,0]]), '/keyhole_subscriber',[0,0,1])
self.add_marker(ecm_pose, '/current_ecm_pose', [1,0,0], Marker.ARROW, scale=[.1,.005,.005])
temp = clean_joints['ecm'].position
b,_ = self.ecm_kin.FK([temp[0],temp[1],.14,temp[3]])
# find the equation of the line that goes through the key_hole and the
# mid_point
ab_vector = (mid_point-key_hole)
ecm_current_direction = b-key_hole
self.add_marker(ecm_pose, '/midpoint_to_keyhole', [0,1,1], type=Marker.LINE_LIST, scale = [0.005, 0.005, 0.005], points=[b, key_hole])
self.add_marker(PoseConv.to_homo_mat([ab_vector,[0,0,0]]), '/ab_vector',[0,1,0], type=Marker.ARROW)
r = self.find_rotation_matrix_between_two_vectors(ecm_current_direction, ab_vector)
m = math.sqrt(ab_vector[0]**2 + ab_vector[1]**2 + ab_vector[2]**2) # ab_vector's length
# insertion joint length
l = math.sqrt( (ecm_pose[0,3]-key_hole[0])**2 + (ecm_pose[1,3]-key_hole[1])**2 + (ecm_pose[2,3]-key_hole[2])**2)
# Equation of the line that passes through the midpoint of the tools and the key hole
x = lambda t: key_hole[0] + ab_vector[0] * t
y = lambda t: key_hole[1] + ab_vector[1] * t
z = lambda t: key_hole[2] + ab_vector[2] * t
t = l/m
new_ecm_position = numpy.array([x(t), y(t), z(t)]).reshape(3,1)
ecm_pose[0:3,0:3] = r* ecm_pose[0:3,0:3]
ecm_pose[0:3,3] = new_ecm_position
self.add_marker(ecm_pose, '/target_ecm_pose', [0,0,1], Marker.ARROW, scale=[.1,.005,.005])
output_msg = clean_joints['ecm']
try:
p = self.ecm_kin.inverse(ecm_pose)
except Exception as e:
rospy.logerr('error')
if p != None:
p[3] = 0
output_msg.position = p
return output_msg
def zoom_fitness(self, cam_info, mid_point, inner_margin, deadzone_margin, tool_point):
x = cam_info.width; y = cam_info.height
# mid_point = [x/2, y/2]
# if tool in inner zone
if abs(tool_point[0]-mid_point[0]) <= inner_margin * x/2 and abs(tool_point[1] - mid_point[1]) < inner_margin * y/2:
r = inner_margin - max([abs(tool_point[0]-mid_point[0])/(x/2),abs(tool_point[1] - mid_point[1])/(y/2)])
return r
# if tool in deadzone
elif abs(tool_point[0]-mid_point[0]) <= deadzone_margin * x/2 and abs(tool_point[1] - mid_point[1]) < deadzone_margin * y/2:
return 0
#if tool in outer zone
elif abs(tool_point[0]-mid_point[0]) <= x/2 and abs(tool_point[1] - mid_point[1]) < y/2:
return -min( [ abs(min([abs(tool_point[0] - x), tool_point[0]])/(x/2) - deadzone_margin), abs(min([abs(tool_point[1] - y), tool_point[1]])/(y/2) - deadzone_margin)])
else:
return -.1
# radius is a percentage of the width of the screen
def zoom_fitness2(self, cam_info, mid_point, tool_point, tool_point2, radius, deadzone_radius):
r = radius * cam_info.width # r is in pixels
dr = deadzone_radius * cam_info.width # dr is in pixels
tool_in_view = lambda tool, safespace : \
tool[0] > safespace or tool[1] > safespace or tool_point[0] < (cam_info.width-safespace) or \
tool[1] < (cam_info.height-safespace)
dist = lambda a,b : norm( [i-j for i,j in zip(a,b)] )
self.logerror(dist(tool_point, tool_point2))
if dist(tool_point, mid_point) < abs(r - dr): # the tool's distance from the mid_point > r
# return positive value
return 0.001 # in meters
elif dist(tool_point, mid_point) > abs(r + dr): # the tool's distance from the mid_point < r
# return a negative value
return -0.001
elif not tool_in_view(tool_point, 20) or not tool_in_view(tool_point2, 20):
return -0.001
else:
return 0
def unrectify_point(self, point, camera):
left_ray = camera.left.projectPixelTo3dRay(point)
t_vec = [0,0,0]
cv2.Rodrigues( camera.left.R)
def get_2d_point_from_3d_point_relative_to_world_rf(self, cam_info, TEW_inv, point):
b = numpy.matrix( [ [-1, 0, 0], [0,1,0], [0,0,-1]])
# point[0:3] = b * point[0:3]
P = numpy.array(cam_info.P).reshape(3,4)
m = TEW_inv * point
u,v,w = P * m
x = u/w
y = v/w
Width = 640; Height = 480
x = x * .5 + .5 * Width;
y = y *.5 + .5 * Height
return ( x,y )
def find_zoom_level(self, msg, cam_info, clean_joints):
if cam_info != None:
T1W = self.psm1_kin.forward(clean_joints['psm1'].position)
T2W = self.psm2_kin.forward(clean_joints['psm2'].position)
TEW = self.ecm_kin.forward(clean_joints['ecm'].position)
TEW_inv = numpy.linalg.inv(TEW)
T1W_inv = numpy.linalg.inv(T1W)
T2W_inv = numpy.linalg.inv(T2W)
mid_point = (T1W[0:4,3] + T2W[0:4,3])/2
p1 = T1W[0:4,3]
p2 = T2W[0:4,3]
T2E = TEW_inv * T2W
# ig = image_geometry.PinholeCameraModel()
ig = image_geometry.StereoCameraModel()
ig.fromCameraInfo(cam_info['right'], cam_info['left'])
# Format in fakecam.launch: x y z yaw pitch roll [fixed-axis rotations: x(roll),y(pitch),z(yaw)]
# Format for PoseConv.to_homo_mat: (x,y,z) (roll, pitch, yaw) [fixed-axis rotations: x(roll),y(pitch),z(yaw)]
r = PoseConv.to_homo_mat( [ (0.0, 0.0, 0.0), (0.0, 0.0, 1.57079632679) ])
r_inv = numpy.linalg.inv(r);
# r = numpy.linalg.inv(r)
self.logerror( r.__str__())
# rotate_vector = lambda x: (r * numpy.array([ [x[0]], [x[1]], [x[2]], [1] ]) )[0:3,3]
self.add_marker(T2W, '/left_arm', scale=[.002,.002,.002])
l1, r1 = ig.project3dToPixel( ( r_inv * TEW_inv * T1W )[0:3,3]) # tool1 left and right pixel positions
l2, r2 = ig.project3dToPixel( ( r_inv * TEW_inv * T2W )[0:3,3]) # tool2 left and right pixel positions
lm, rm = ig.project3dToPixel( ( r_inv * TEW_inv * mid_point)[0:3,0]) # midpoint left and right pixel positions
# add_100 = lambda x : (x[0] *.5 + cam_info.width/2, x[1])
# l1 = add_100(l1)
# l2 = add_100(l2)
# lm = add_100(lm)
self.zoom_level_positions = {'l1':l1, 'r1':r1, 'l2':l2, 'r2':r2, 'lm':lm, 'rm':rm}
test1_l, test1_r = ig.project3dToPixel( [1,0,0])
test2_l, test2_r = ig.project3dToPixel( [0,0,1])
self.logerror('\ntest1_l = ' + test1_l.__str__() + '\ntest2_l = ' + test2_l.__str__() )
# logerror('xm=%f,'%lm[0] + 'ym=%f'%lm[1])
msg.position[3] = 0
# zoom_percentage = zoom_fitness(cam_info=cam_info, mid_point=[xm, ym], inner_margin=.20,
# deadzone_margin= .70, tool_point= [x1,y1])
zoom_percentage = self.zoom_fitness2(cam_info['left'], mid_point=lm, tool_point=l1,
tool_point2=l2, radius=.1, deadzone_radius=.01)
msg.position[2] = msg.position[2] + zoom_percentage
if msg.position[2] < 0 : # minimum 0
msg.position[2] = 0.00
elif msg.position[2] > .21: # maximum .23
msg.position[2] = .21
return msg
def compute_viewangle(self, joint, cam_info):
kinematics = lambda name: self.psm1_kin if name == 'psm1' else self.psm2_kin if name == 'psm2' else self.ecm_kin
clean_joints = {}
try:
joint_names = joint.keys()
for j in joint_names:
clean_joints[j] = self.extract_positions(joint[j], j, kinematics(j))
key_hole, _ = self.ecm_kin.FK([0,0,0,0]) # The position of the keyhole, is the end-effector's
psm1_pos,_ = self.psm1_kin.FK(clean_joints['psm1'].position)
psm2_pos,_ = self.psm2_kin.FK(clean_joints['psm2'].position)
psm1_pose = self.psm1_kin.forward(clean_joints['psm1'].position)
ecm_pose = self.ecm_kin.forward(clean_joints['ecm'].position)
except Exception as e:
# rospy.logerr(e.message)
output_msg = joint['ecm']
# output_msg.name = ['outer_yaw', 'outer_pitch', 'insertion', 'outer_roll']
# output_msg.position = [joint['ecm'].position[x] for x in [0,1,5,6]]
return output_msg
output_msg = clean_joints['ecm']
gripper= max( [ abs(joint['psm1'].position[-1]), abs(joint['psm2'].position[-1])] ) < math.pi/8
# rospy.logerr('psm1 gripper = ' + joint['psm1'].position[-1].__str__() + 'gripper = ' + gripper.__str__())
if gripper == gripper or gripper != gripper: # luke was here
output_msg = self.point_towards_midpoint(clean_joints, psm1_pos, psm2_pos, key_hole, ecm_pose)
output_msg = self.find_zoom_level(output_msg, cam_info, clean_joints)
pass
if len(output_msg.name) > 4:
output_msg.name = ['outer_yaw', 'outer_pitch', 'insertion', 'outer_roll']
if len(output_msg.position) >= 7:
output_msg.position = [output_msg.position[x] for x in [0,1,5,6]]
self.logerror(output_msg.__str__())
return output_msg
class BallWatcher(object):
def __init__(self):
rospy.loginfo("Creating BallWatcher class")
self.print_help()
self.objDetector = Obj3DDetector()
# flags and vars
# for ball dropping position
self.kinect_calibrate_flag = False
self.running_flag = False
self.start_calc_flag = False
self.ball_marker = sawyer_mk.MarkerDrawer("/base", "ball", 500)
self.drop_point = Point()
self.drop_point_arr = []
self.pos_rec_list = np.array([])
self.drop_point_marker = sawyer_mk.MarkerDrawer(
"/base", "dropping_ball", 500)
self.ik_cont = IKController()
self.robot = URDF.from_parameter_server()
self.kin = KDLKinematics(self.robot, BASE, EE_FRAME)
self.limb_interface = intera_interface.Limb()
self.pos_rec = [
PointStamped() for i in range(2)
] # array 1x2 with each cell storing a PointStamped with cartesian position of the ball in the past two frames
self.old_pos_rec = [PointStamped() for i in range(2)]
self.last_tf_time = rospy.Time()
self.tf_listener = tf.TransformListener()
self.tf_bc = tf.TransformBroadcaster()
self.tf_listener.waitForTransform("base", "camera_link", rospy.Time(),
rospy.Duration(0.5))
self.T_sc_p, self.T_sc_q = self.tf_listener.lookupTransform(
"base", "camera_link", rospy.Time())
self.T_sc = tr.euler_matrix(*tr.euler_from_quaternion(self.T_sc_q))
self.T_sc[0:3, -1] = self.T_sc_p
# kbhit instance
self.kb = kbhit.KBHit()
rospy.on_shutdown(self.kb.set_normal_term)
# publishers and timers
self.kb_timer = rospy.Timer(rospy.Duration(0.1), self.keycb)
self.tf_timer = rospy.Timer(rospy.Duration(0.01), self.tf_update_cb)
self.final_x_total = 0
self.final_y_total = 0
def check_start_throw(self):
# check if the ball is being thrown, by comparing between z of the first and second frame
z_past = self.pos_rec[0].point.z
z_present = self.pos_rec[1].point.z
if (z_present - z_past > 0.05):
self.start_calc_flag = True
def check_stop_throw(self):
# check if the ball reach the end of trajectory, if so, raise down the start_calc_flag
x_past = self.pos_rec[0].point.x
x_present = self.pos_rec[1].point.x
if (x_present - x_past > 0):
self.start_calc_flag = False
self.pos_rec_list = np.array([])
# print "test"
def roll_mat(self, mat):
row_num = len(mat)
for i in range(0, row_num - 1):
mat[i] = mat[i + 1]
return
def tf_update_cb(self, tdat):
# update ball position in real time
# Filter ball outside x = 0 - 3.0m relative to base out
self.p_cb = self.objDetector.p_ball_to_cam
# print self.p_cb
if (self.p_cb == []):
# print "Please wait, the system is detecting the ball"
return
# try:
# self.tf_listener.waitForTransform(TARGET_FRAME, SOURCE_FRAME, rospy.Time(), rospy.Duration(0.5))
# except tf.Exception:
# rospy.loginfo("No frame of /ball from /base received, stop calculation. self.start_calc_flag = False")
# p, q = self.tf_listener.lookupTransform(TARGET_FRAME, SOURCE_FRAME, rospy.Time())
# change to subscribe lookuptf directly and multiply directly
# P_cb = copy.deepcopy(self.objDetector.p_ball_to_cam)
P_cb = np.array([
self.objDetector.p_ball_to_cam[0],
self.objDetector.p_ball_to_cam[1],
self.objDetector.p_ball_to_cam[2],
self.objDetector.p_ball_to_cam[3]
])
timestamp = P_cb[3]
P_cb[3] = 1
P_cb = P_cb.reshape((4, 1))
p_sb = np.dot(self.T_sc, P_cb)
self.tf_bc.sendTransform((p_sb[0][0], p_sb[1][0], p_sb[2][0]),
[0, 0, 0, 1], rospy.Time.now(), "ball",
"base")
pos = PointStamped()
pos.header.stamp = timestamp
# pos.point.x = p[0]
# pos.point.y = p[1]
# pos.point.z = p[2]
# print "1: ", p_sb[0][0]
# print "2: ", p_sb[1][0]
# print "3: ", p_sb[2][0]
pos.point.x = p_sb[0][0]
pos.point.y = p_sb[1][0]
pos.point.z = p_sb[2][0]
# filter repeated received tf out
if (self.pos_rec[-1].header.stamp != pos.header.stamp) and (
self.pos_rec[-1].point.x !=
pos.point.x) and (pos.point.x < 3.5) and (
pos.point.x - self.pos_rec[-1].point.x < 1.2):
self.roll_mat(self.pos_rec)
self.pos_rec[-1] = pos
# choose only a non-repeated pos_rec by comparing between the timestamped of the present and past pos_rec
if (self.last_tf_time != self.pos_rec[0].header.stamp):
# If running_flag is True, start detecting
if self.running_flag:
# check if the ball is being thrown yet
if not self.start_calc_flag:
self.check_start_throw()
self.ball_marker.draw_spheres([0, 0.7, 0, 1],
[0.03, 0.03, 0.03],
self.pos_rec[0].point)
self.ball_marker.draw_line_strips([5.7, 1, 4.7, 1],
[0.01, 0, 0],
self.pos_rec[0].point,
self.pos_rec[1].point)
if self.start_calc_flag:
self.check_stop_throw()
if not self.start_calc_flag:
return
# draw markers
self.ball_marker.draw_spheres([0.7, 0, 0, 1],
[0.03, 0.03, 0.03],
self.pos_rec[0].point)
self.ball_marker.draw_line_strips([1, 0.27, 0.27, 1],
[0.01, 0, 0],
self.pos_rec[0].point,
self.pos_rec[1].point)
self.ball_marker.draw_numtxts([1, 1, 1, 1], 0.03,
self.pos_rec[0].point, 0.03)
# calculate the dropping position based on 2 points
print "##########"
print "pos_rec_listB4: ", self.pos_rec_list
self.pos_rec_list = np.append(self.pos_rec_list,
self.pos_rec[0])
# self.pos_rec_list = (self.pos_rec_list).append(self.pos_rec[0])
print "pos_rec_list: ", self.pos_rec_list
print "##########"
self.drop_point = sawyer_calc.opt_min_proj_calc(
self.pos_rec_list, Z_CENTER)
# average drop point
input_posestamped = PoseStamped()
input_posestamped.pose.position = self.drop_point
input_posestamped.pose.orientation = Quaternion(
0.0392407571798, 0.664506667783, -0.0505321422468,
0.744538483926)
if self.pos_rec_list.shape[0] >= 4:
self.ik_cont.running_flag = True
self.ik_cont.set_goal_from_pose(input_posestamped)
self.drop_point_marker.draw_spheres([0, 0, 0.7, 1],
[0.03, 0.03, 0.03],
self.drop_point)
self.drop_point_marker.draw_numtxts([1, 1, 1, 1], 0.03,
self.drop_point, 0.03)
self.last_tf_time = self.pos_rec[0].header.stamp
def keycb(self, tdat):
# check if there was a key pressed, and if so, check it's value and
# toggle flag:
if self.kb.kbhit():
c = self.kb.getch()
if ord(c) == 27:
rospy.signal_shutdown("Escape pressed!")
else:
print c
if c == 's':
rospy.loginfo(
"You pressed 's', Program starts. Sawyer is waiting for the ball to be thrown."
)
self.running_flag = not self.running_flag
if not self.running_flag:
self.ball_marker.delete_all_mks()
self.pos_rec_list = []
self.ik_cont.running_flag = False
elif c == 'c':
rospy.loginfo(
"You pressed 'c', Start calibration\nrunning_flag = False")
self.running_flag = False
self.kinect_pos_calib()
elif c == 'h':
rospy.loginfo(
"You pressed 'h', Go to home position\nrunning_flag = False"
)
self.running_flag = False
self.home_pos()
elif c == 'p':
rospy.loginfo("You pressed 'h', Plotting")
self.plot_flag = True
else:
self.print_help()
self.kb.flush()
return
def kinect_pos_calib(self):
#
if not self.kinect_calibrate_flag:
# input x,y,z and x,y,z
p = [X_KINECT_CALIBR, Y_KINECT_CALIBR, Z_KINECT_CALIBR]
self.kinect_calibrate_flag = True
rospy.loginfo(
"Press C again for calibrate the height of the camera")
else:
p = [X_KINECT_CALIBR_2, Y_KINECT_CALIBR, Z_KINECT_CALIBR]
self.kinect_calibrate_flag = False
G = tr.euler_matrix(ROLL_KINECT_CALIBR, PITCH_KINECT_CALIBR,
YAW_KINECT_CALIBR)
for i in range(3):
G[i][3] = p[i]
q_seed = self.kin.random_joint_angles()
j_names = self.kin.get_joint_names()
q = self.kin.inverse(G, q_seed)
while (q is None):
q_seed = self.kin.random_joint_angles()
q = self.kin.inverse(G, q_seed)
j_req = dict(zip(j_names, q))
self.limb_interface.move_to_joint_positions(j_req)
def home_pos(self):
self.limb_interface.move_to_joint_positions({
'head_pan': 0.379125,
'right_j0': -0.020025390625,
'right_j1': 0.8227529296875,
'right_j2': -2.0955126953125,
'right_j3': 2.172509765625,
'right_j4': 0.7021171875,
'right_j5': -1.5003603515625,
'right_j6': -2.204990234375
})
def print_help(self):
help_string = \
"""
's' ~ Start the program
'c' ~ Calibration the kinect position
'h' ~ Move to home position
'p' ~ Plot the trajectory of each Cartesian coordinate
'ESC' ~ Quit
"""
print help_string
return
def __init__(self, hebi_group_name, hebi_mapping, leg_base_links,
leg_end_links):
rospy.loginfo("Creating Hexapod instance...")
hebi_families = []
hebi_names = []
for leg in hebi_mapping:
for actuator in leg:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_mapping = hebi_mapping
self.hebi_mapping_flat = self._flatten(self.hebi_mapping)
# jt information populated by self._feedback_cb
self._current_jt_pos = {}
self._current_jt_vel = {}
self._current_jt_eff = {}
self._joint_state_pub = None
self._hold_leg_list = [False, False, False, False, False, False]
self._hold_leg_positions = self._get_list_of_lists()
# Create a service client to create a group
set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names',
AddGroupFromNamesSrv)
# Topic to receive feedback from a group
self.hebi_group_feedback_topic = "/hebiros/" + hebi_group_name + "/feedback/joint_state"
rospy.loginfo(" hebi_group_feedback_topic: %s",
"/hebiros/" + hebi_group_name + "/feedback/joint_state")
# Topic to send commands to a group
self.hebi_group_command_topic = "/hebiros/" + hebi_group_name + "/command/joint_state"
rospy.loginfo(" hebi_group_command_topic: %s",
"/hebiros/" + hebi_group_name + "/command/joint_state")
# Call the /hebiros/add_group_from_names service to create a group
rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...",
'/hebiros/add_group_from_names')
rospy.wait_for_service('/hebiros/add_group_from_names'
) # block until service server starts
rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.")
set_hebi_group(hebi_group_name, hebi_names, hebi_families)
# Create a service client to set group settings
change_group_settings = rospy.ServiceProxy(
"/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement",
SendCommandWithAcknowledgementSrv)
rospy.loginfo(
" Waiting for SendCommandWithAcknowledgementSrv at %s ...",
"/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement")
rospy.wait_for_service("/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement"
) # block until service server starts
cmd_msg = CommandMsg()
cmd_msg.name = self.hebi_mapping_flat
cmd_msg.settings.name = self.hebi_mapping_flat
cmd_msg.settings.position_gains.name = self.hebi_mapping_flat
cmd_msg.settings.position_gains.kp = [5, 8, 2] * LEGS
cmd_msg.settings.position_gains.ki = [0.001] * LEGS * ACTUATORS_PER_LEG
cmd_msg.settings.position_gains.kd = [0] * LEGS * ACTUATORS_PER_LEG
cmd_msg.settings.position_gains.i_clamp = [
0.25
] * LEGS * ACTUATORS_PER_LEG # TODO: Tune this. Setting it low for testing w/o restarting Gazebo
change_group_settings(cmd_msg)
# Feedback/Command
self.fbk_sub = rospy.Subscriber(self.hebi_group_feedback_topic,
JointState, self._feedback_cb)
self.cmd_pub = rospy.Publisher(self.hebi_group_command_topic,
JointState,
queue_size=1)
self._hold_position = False
self._hold_joint_states = {}
# TrajectoryAction client
self.trajectory_action_client = SimpleActionClient(
"/hebiros/" + hebi_group_name + "/trajectory", TrajectoryAction)
rospy.loginfo(" Waiting for TrajectoryActionServer at %s ...",
"/hebiros/" + hebi_group_name + "/trajectory")
self.trajectory_action_client.wait_for_server(
) # block until action server starts
rospy.loginfo(" TrajectoryActionServer AVAILABLE.")
# Twist Subscriber
self._cmd_vel_sub = rospy.Subscriber("/hexapod/cmd_vel/", Twist,
self._cmd_vel_cb)
self.last_vel_cmd = None
self.linear_displacement_limit = 0.075 # m
self.angular_displacement_limit = 0.65 # rad
# Check ROS Parameter server for robot_description URDF
urdf_str = ""
urdf_loaded = False
while not rospy.is_shutdown() and not urdf_loaded:
if rospy.has_param('/robot_description'):
urdf_str = rospy.get_param('/robot_description')
urdf_loaded = True
rospy.loginfo(
"Pulled /robot_description from parameter server.")
else:
rospy.sleep(0.01) # sleep for 10 ms of ROS time
# pykdl_utils setup
self.robot_urdf = URDF.from_xml_string(urdf_str)
self.kdl_fk_base_to_leg_base = [
KDLKinematics(self.robot_urdf, 'base_link', base_link)
for base_link in leg_base_links
]
self.kdl_fk_leg_base_to_eff = [
KDLKinematics(self.robot_urdf, base_link, end_link)
for base_link, end_link in zip(leg_base_links, leg_end_links)
]
# trac-ik setup
self.trac_ik_leg_base_to_end = [
IK(
base_link,
end_link,
urdf_string=urdf_str,
timeout=0.01, # TODO: Tune me
epsilon=1e-4,
solve_type="Distance")
for base_link, end_link in zip(leg_base_links, leg_end_links)
]
self.ik_pos_xyz_bounds = [0.01, 0.01, 0.01]
self.ik_pos_wxyz_bounds = [31416.0, 31416.0, 31416.0
] # NOTE: This implements position-only IK
# Wait for connections to be set up
rospy.loginfo("Wait for ROS connections to be set up...")
while not rospy.is_shutdown() and len(self._current_jt_pos) < len(
self.hebi_mapping_flat):
rospy.sleep(0.1)
# Set up joint state publisher
self._joint_state_pub = rospy.Publisher('/joint_states',
JointState,
queue_size=1)
self._loop_rate = rospy.Rate(20)
# leg joint home pos Hip, Knee, Ankle
self.leg_jt_home_pos = [
[0.0, +0.26, -1.57], # Leg 1
[0.0, -0.26, +1.57], # Leg 2
[0.0, +0.26, -1.57], # Leg 3
[0.0, -0.26, +1.57], # Leg 4
[0.0, +0.26, -1.57], # Leg 5
[0.0, -0.26, +1.57]
] # Leg 6
# leg end-effector home position
self.leg_eff_home_pos = self._get_leg_base_to_eff_fk(
self.leg_jt_home_pos)
# leg step height relative to leg base link
self.leg_eff_step_height = [[]] * LEGS # relative to leg base
for i, fk_solver in enumerate(self.kdl_fk_base_to_leg_base):
base_to_leg_base_rot = fk_solver.forward([])[:3, :3]
step_ht_chassis = np.array([0, 0, STEP_HEIGHT])
step_ht_leg_base = np.dot(base_to_leg_base_rot, step_ht_chassis)
self.leg_eff_step_height[i] = step_ht_leg_base.tolist()[0]
self._odd_starts = True
rospy.loginfo("Done creating Hexapod instance...")
class Robot(object):
def __init__(self, sim=True):
f = file('/home/hirolab/catkin_ws/src/vspgrid/urdf/widowx2.urdf', 'r')
robot = URDF.from_xml_string(f.read()) # parsed URDF
# robot = URDF.from_parameter_server()
self.kin = KDLKinematics(robot, 'base', 'mconn')
# x, y, z, yaw, pitch
self.cart_from_matrix = lambda T: np.array([
T[0, 3],
T[1, 3],
T[2, 3],
math.atan2(T[1, 0], T[0, 0]),
-math.asin(T[2, 0])])
self.home = np.array([0.05, 0, 0.2, 0, 0])
self.q = np.array([0, 0, 0, 0, 0])
self.dt = 0.05
self.sim = sim
def publish(eventStop):
# for arbotix
pub1 = rospy.Publisher("q1/command", Float64, queue_size=5)
pub2 = rospy.Publisher("q2/command", Float64, queue_size=5)
pub3 = rospy.Publisher("q3/command", Float64, queue_size=5)
pub4 = rospy.Publisher("q4/command", Float64, queue_size=5)
pub5 = rospy.Publisher("q5/command", Float64, queue_size=5)
# for visualization
jointPub = rospy.Publisher(
"/joint_states", JointState, queue_size=5)
jmsg = JointState()
jmsg.name = ['q1', 'q2', 'q3', 'q4', 'q5']
while not rospy.is_shutdown() and not eventStop.is_set():
jmsg.header.stamp = rospy.Time.now()
jmsg.position = self.q
if self.sim:
jointPub.publish(jmsg)
pub1.publish(self.q[0])
pub2.publish(self.q[1])
pub3.publish(self.q[2])
pub4.publish(self.q[3])
pub5.publish(self.q[4])
eventStop.wait(self.dt)
rospy.init_node("robot")
eventStop = threading.Event()
threadJPub = threading.Thread(target=publish, args=(eventStop,))
threadJPub.daemon = True
threadJPub.start()
def forward(self, q=None):
if q is None:
q = self.q
return(self.kin.forward(q))
def cart(self, q=None):
if q is None:
q = self.q
return(self.cart_from_matrix(self.forward(q)))
def jacobian(self, q=None):
if q is None:
q = self.q
eps = 1e-3
s0 = np.array(self.cart(q))
dof = len(self.q)
J = np.array(dof * [dof * [0.0]])
for k in range(0, dof):
dq = np.zeros(dof)
dq[k] = eps
J[:, k] = (self.cart(q + dq) - s0) / eps
return J
def move(self, s1, speed=1):
def plan(s1):
# return tuples of joint coordinates to be moved at every self.dt
# first calculate goal_joint
def error_function(q): return (
np.array(self.cart(q)) - np.array(s1))
x, _, ler, message = scipy.optimize.fsolve(
error_function, 0*self.q, full_output=True)
if not ler == 1:
# print "No solution found. Target: ", goal_space
# res = minimize(lambda q: np.linalg.norm(error_function(q)), self.q*0)
# x = res.x
# if not res.success:
# raise Exception('No IK solution found')
raise Exception('No IK solution found')
q1 = (x + math.pi) % (2 * math.pi) - math.pi
# q1 = x
# break trajectory to intermediate points
q0 = self.q
s0 = self.cart(q0)
t1 = np.max(np.abs(q1 - q0)) / speed
time = np.arange(0, t1, self.dt) + self.dt
dof = len(self.q)
qq = np.array([np.interp(time, [0, t1], [q0[k], q1[k]])
for k in range(dof)]).transpose()
return qq
traj = plan(s1)
rate = rospy.Rate(1.0 / self.dt)
for joints in traj:
self.q = joints
rate.sleep()
def move2(self, s1):
speed = 1.5
TOL = 1e-8
MAX_VEL = 0.2
t0 = rospy.Time.now()
while rospy.Time.now() - t0 < rospy.Duration(5):
ds = s1 - self.cart()
dev = np.linalg.norm(ds)
if dev < TOL:
print("Success. Deviation: ")
print(dev)
return
J = self.jacobian()
dq = np.linalg.solve(J, ds)
if max(abs(dq)) > MAX_VEL:
dq = dq * MAX_VEL / max(abs(dq))
alfa = speed * self.dt * (rospy.Time.now() - t0).to_sec()**1.5
self.q = self.q + dq * alfa
rospy.sleep(self.dt)
print("Failure. Deviation: ")
print(dev)
def reset(self):
self.move(self.cart(self.q*0))
def __init__(self,
base_link, end_link, planning_group,
world="/world",
listener=None,
traj_step_t=0.1,
max_acc=1,
max_vel=1,
max_goal_diff = 0.02,
goal_rotation_weight = 0.01,
max_q_diff = 1e-6,
start_js_cb=True,
base_steps=10,
steps_per_meter=100):
self.world = world
self.base_link = base_link
self.end_link = end_link
self.planning_group = planning_group
self.base_steps = base_steps
self.steps_per_meter = steps_per_meter
self.MAX_ACC = max_acc
self.MAX_VEL = max_vel
self.traj_step_t = traj_step_t
self.max_goal_diff = max_goal_diff
self.max_q_diff = max_q_diff
self.goal_rotation_weight = goal_rotation_weight
self.at_goal = True
self.near_goal = True
self.moving = False
self.q0 = [0,0,0,0,0,0,0]
self.old_q0 = [0,0,0,0,0,0,0]
self.cur_stamp = 0
self.teach_mode = rospy.Service('/costar/SetTeachMode',SetTeachMode,self.set_teach_mode_call)
self.servo_mode = rospy.Service('/costar/SetServoMode',SetServoMode,self.set_servo_mode_call)
self.shutdown = rospy.Service('/costar/ShutdownArm',EmptyService,self.shutdown_arm_call)
self.servo = rospy.Service('/costar/ServoToPose',ServoToPose,self.servo_to_pose_call)
self.plan = rospy.Service('/costar/PlanToPose',ServoToPose,self.plan_to_pose_call)
self.smartmove = rospy.Service('/costar/SmartMove',SmartMove,self.smart_move_call)
self.js_servo = rospy.Service('/costar/ServoToJointState',ServoToJointState,self.servo_to_joints_call)
self.pt_publisher = rospy.Publisher('/joint_traj_pt_cmd',JointTrajectoryPoint,queue_size=1000)
self.get_waypoints_srv = GetWaypointsService(world=world,service=False)
self.driver_status = 'IDLE'
self.status_publisher = rospy.Publisher('/costar/DriverStatus',String,queue_size=1000)
self.robot = URDF.from_parameter_server()
if start_js_cb:
self.js_subscriber = rospy.Subscriber('joint_states',JointState,self.js_cb)
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(base_link, end_link)
if listener is None:
self.listener = tf.TransformListener()
else:
self.listener = listener
#print self.tree.getNrOfSegments()
#print self.chain.getNrOfJoints()
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
self.display_pub = rospy.Publisher('costar/display_trajectory',DisplayTrajectory,queue_size=1000)
#self.set_goal(self.q0)
self.goal = None
self.ee_pose = None
self.planner = SimplePlanning(self.robot,base_link,end_link,self.planning_group)
class OmniMiniProj:
def __init__(self):
#Create a tf listener
self.tfListener = tf.TransformListener()
#tf.TransfromListener is a subclass that subscribes to the "/tf" message topic and adds transform
#data to the tf data structure. As a result the object is up to date with all current transforms
#Create a publisher to a new topic name called "stylus_to_base_transf with a message type Twist"
self.Tsb = rospy.Publisher('stylus_to_base_transf',Twist)
#Find the omni from the parameter server
self.omni = URDF.from_parameter_server()
#Note: A node that initializes and runs the Phantom Omni has to be running in the background for
# from_parameter_server to to find the parameter.
# Older versions of URDF label the function "load_from_parameter_server"
#Subscribe to the "omni1_joint_states" topic, which provides the joint states measured by the omni in a
# ROS message of the type sensor_msgs/JointState. Every time a message is published to that topic run the
#callback function self.get_joint_states, which is defined below.
self.joint_state_sub = rospy.Subscriber("omni1_joint_states", JointState, self.get_joint_states)
#OmniMiniProjTimers
#Run a timer to manipulate the class structure as needed. The timer's
#callback function "timer_callback" then calls the desired functions
self.timer1 = rospy.Timer(rospy.Duration(0.01), self.timer_callback)
#Create a separate slower timer on a lower frequency for a comfortable print out
self.print_timer = rospy.Timer(rospy.Duration(1), self.print_output)
return
#The timer_callback runs every time timer1 triggers. In this example, it merely calls other functions. Generally,
#it may include further calculations, just like any regular function.
def timer_callback(self, data):
self.tf_base_to_stylus()
return
#get_joint_states extracts the joint agles from the JointState message (angles, agular velocity, ...), which is
#provided by the 'joint_state_sub' subscriber.
def get_joint_states(self,data):
try:
q_sensors = data.position
except rospy.ROSInterruptException:
self.q_sensors = None
pass
if q_sensors != None:
self.kdl_kinematics(q_sensors)
return
#tf_base_to_stylus uses tf to look up the transfrom from the frame located at the root of the Omni's URDF (/base) to the frame
#at the end of the URDF (/stylus).
def tf_base_to_stylus (self):
try:
(self.transl,self.quat) = self.tfListener.lookupTransform('base','stylus',rospy.Time(0))
#lookupTransrom is a method which returns the transfrom between two coordinate frames.
#lookupTransfrom returns a translation and a quaternion
self.rot= euler_from_quaternion(self.quat) #Get euler angles from the quaternion
self.tf_SE3 = compose_matrix(angles=self.rot,translate=self.transl)
#Store the transformation in a format compatible with gemoetr_msgs/Twist
self.transf = Twist(Vector3(self.transl[0],self.transl[1],self.transl[2]),(Vector3(self.rot[0],self.rot[1],self.rot[2])))
#Publish the transformation
self.Tsb.publish(self.transf)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
self.tf_SE3 = None
pass
#If exceptions occur, skip trying to lookup the transform.
#It is encouraged to publish a logging message to rosout with rospy.
#i.e:
#rospy.logerr("Could not transform from %s to %s,"base","stylus")
return
def kdl_kinematics (self,data):
self.q_sensors = data
self.tree = kdl_tree_from_urdf_model(self.omni) # create a kdl tree from omni URDF model
self.omni_kin = KDLKinematics(self.omni, "base", "stylus") # create instance that captures the kinematics of the robot arm
#Forward Kinematics
self.pose_stylus = self.omni_kin.forward(data) #compute the forward kinematics from the sensor joint angle position using the kinematics from the kdl tree
#Inverse Kinematics
self.q_guess = numpy.array(data)+0.2 #make an initial guess for your joint angles by deviating all the sensor joint angles by 0.2
self.q_ik = self.omni_kin.inverse(self.pose_stylus, self.q_guess) #using the position from the forward kinematics 'pose_stylus' and the offset initial guess, compute
#the desired joint angles for that position.
self.delta_q = self.q_ik-data
def print_output (self,data):
if self.tf_SE3 != None and self.q_sensors !=None:
print "Base to stylus using tf:","\n", self.tf_SE3, "\n"
print "base to stylus using KDL forward kinematics", "\n" , self.pose_stylus, "\n"
print "transformations.is.same_transform tf vs KDL forward kin:", "\n", is_same_transform(self.tf_SE3,self.pose_stylus), "\n"
print "Joint angles from sensors q_sensors:","\n" , self.q_sensors, "\n"
print "Joint state angles from KDL inverse kinematics q_ik", "\n" , self.q_ik , "\n"
print "The difference delta_q between q_sensors and q_ik", "\n" , self.delta_q , "\n\n\n\n\n\n\n\n\n"
return
class CostarArm(object):
def __init__(self,
base_link, end_link, planning_group,
world="/world",
listener=None,
traj_step_t=0.1,
max_acc=1,
max_vel=1,
max_goal_diff = 0.02,
goal_rotation_weight = 0.01,
max_q_diff = 1e-6,
start_js_cb=True,
base_steps=10,
steps_per_meter=100):
self.world = world
self.base_link = base_link
self.end_link = end_link
self.planning_group = planning_group
self.base_steps = base_steps
self.steps_per_meter = steps_per_meter
self.MAX_ACC = max_acc
self.MAX_VEL = max_vel
self.traj_step_t = traj_step_t
self.max_goal_diff = max_goal_diff
self.max_q_diff = max_q_diff
self.goal_rotation_weight = goal_rotation_weight
self.at_goal = True
self.near_goal = True
self.moving = False
self.q0 = [0,0,0,0,0,0,0]
self.old_q0 = [0,0,0,0,0,0,0]
self.cur_stamp = 0
self.teach_mode = rospy.Service('/costar/SetTeachMode',SetTeachMode,self.set_teach_mode_call)
self.servo_mode = rospy.Service('/costar/SetServoMode',SetServoMode,self.set_servo_mode_call)
self.shutdown = rospy.Service('/costar/ShutdownArm',EmptyService,self.shutdown_arm_call)
self.servo = rospy.Service('/costar/ServoToPose',ServoToPose,self.servo_to_pose_call)
self.plan = rospy.Service('/costar/PlanToPose',ServoToPose,self.plan_to_pose_call)
self.smartmove = rospy.Service('/costar/SmartMove',SmartMove,self.smart_move_call)
self.js_servo = rospy.Service('/costar/ServoToJointState',ServoToJointState,self.servo_to_joints_call)
self.pt_publisher = rospy.Publisher('/joint_traj_pt_cmd',JointTrajectoryPoint,queue_size=1000)
self.get_waypoints_srv = GetWaypointsService(world=world,service=False)
self.driver_status = 'IDLE'
self.status_publisher = rospy.Publisher('/costar/DriverStatus',String,queue_size=1000)
self.robot = URDF.from_parameter_server()
if start_js_cb:
self.js_subscriber = rospy.Subscriber('joint_states',JointState,self.js_cb)
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(base_link, end_link)
if listener is None:
self.listener = tf.TransformListener()
else:
self.listener = listener
#print self.tree.getNrOfSegments()
#print self.chain.getNrOfJoints()
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
self.display_pub = rospy.Publisher('costar/display_trajectory',DisplayTrajectory,queue_size=1000)
#self.set_goal(self.q0)
self.goal = None
self.ee_pose = None
self.planner = SimplePlanning(self.robot,base_link,end_link,self.planning_group)
'''
js_cb
listen to robot joint state information
'''
def js_cb(self,msg):
self.old_q0 = self.q0
self.q0 = np.array(msg.position)
self.update_position()
'''
update current position information
'''
def update_position(self):
self.ee_pose = pm.fromMatrix(self.kdl_kin.forward(self.q0))
if self.goal is not None:
#goal_diff = np.abs(self.goal - self.q0).sum() / self.q0.shape[0]
cart_diff = (self.ee_pose.p - self.goal.p).Norm()
rot_diff = self.goal_rotation_weight * (pm.Vector(*self.ee_pose.M.GetRPY()) - pm.Vector(*self.goal.M.GetRPY())).Norm()
goal_diff = cart_diff + rot_diff
if goal_diff < self.max_goal_diff:
self.at_goal = True
if goal_diff < 10*self.max_goal_diff:
self.near_goal = True
q_diff = np.abs(self.old_q0 - self.q0).sum()
if q_diff < self.max_q_diff:
self.moving = False
else:
self.moving = True
def check_req_speed_params(self,req):
if req.accel > self.MAX_ACC:
acceleration = self.MAX_ACC
else:
acceleration = req.accel
if req.vel > self.MAX_VEL:
velocity = self.MAX_VEL
else:
velocity = req.vel
return (acceleration, velocity)
'''
Find any valid object that meets the requirements.
Find a cartesian path or possibly longer path through joint space.
'''
def smart_move_call(self,req):
if False and not self.driver_status == 'SERVO':
rospy.logerr('DRIVER -- Not in servo mode!')
return 'FAILURE - not in servo mode'
(acceleration, velocity) = self.check_req_speed_params(req)
(poses,names) = self.get_waypoints_srv.get_waypoints(
req.obj_class, # object class to move to
req.predicates, # predicates to match
[req.pose], # offset/transform from each member of the class
["tmp"] # placeholder name
)
print req.obj_class
print req.predicates
print names
print poses
if not poses is None:
msg = 'FAILURE - no valid objects found!'
qs = []
dists = []
for (pose,name) in zip(poses,names):
# figure out which tf frame we care about
tf_path = name.split('/')
tf_frame = ''
for part in tf_path:
if len(part) > 0:
tf_frame = part
break
if len(tf_frame) == 0:
continue
# try to move to the pose until one succeeds
T_base_world = pm.fromTf(self.listener.lookupTransform(self.world,self.base_link,rospy.Time(0)))
T = T_base_world.Inverse()*pm.fromMsg(pose)
# Check acceleration and velocity limits
# Send command
pt = JointTrajectoryPoint()
#q = self.planner.ik(T, self.q0)
traj = self.planner.getCartesianMove(T,self.q0,self.base_steps,self.steps_per_meter)
#if len(traj.points) == 0:
# print T
# (code,res) = self.planner.getPlan(pm.toMsg(T),self.q0) # find a non-local movement
# traj = res.planned_trajectory.joint_trajectory
print "Considering object with name = %s"%tf_frame
if len(traj.points) > 0:
qs.append(traj)
dists.append((traj.points[-1].positions - self.q0).sum())
else:
rospy.logwarn('SIMPLE DRIVER -- IK failed for %s'%name)
if len(qs) == 0:
msg = 'FAILURE - no joint configurations found!'
possible_goals = zip(dists,qs)
possible_goals.sort()
print "POSSIBLE GOALS"
print possible_goals
(dist,traj) = possible_goals[0]
rospy.logwarn("Trying to move to frame at distance %f"%(dist))
msg = self.send_trajectory(traj,acceleration,velocity,cartesian=True)
return msg
else:
msg = 'FAILURE - no match to predicate moves'
return msg
def set_goal(self,q):
self.at_goal = False
self.near_goal = False
self.goal = pm.fromMatrix(self.kdl_kin.forward(q))
'''
Definitely do a planned motion.
'''
def plan_to_pose_call(self,req):
#rospy.loginfo('Recieved servo to pose request')
#print req
if self.driver_status == 'SERVO':
T = pm.fromMsg(req.target)
# Check acceleration and velocity limits
(acceleration, velocity) = self.check_req_speed_params(req)
# Send command
pt = JointTrajectoryPoint()
traj = self.planner.getCartesianMove(T,self.q0,self.base_steps,self.steps_per_meter)
if len(traj.points) > 0:
(code,res) = self.planner.getPlan(req.target,traj.points[-1].positions)
else:
(code,res) = self.planner.getPlan(req.target,self.q0)
#pt.positions = q
if code > 0:
self.at_goal = True
return 'SUCCESS - at goal'
if (not res is None) and len(res.planned_trajectory.joint_trajectory.points) > 0:
disp = DisplayTrajectory()
disp.trajectory.append(res.planned_trajectory)
disp.trajectory_start = res.trajectory_start
self.display_pub.publish(disp)
traj = res.planned_trajectory.joint_trajectory
return self.send_trajectory(traj,acceleration,velocity,cartesian=True)
else:
rospy.logerr(res)
rospy.logerr('DRIVER -- PLANNING failed')
return 'FAILURE - not in servo mode'
else:
rospy.logerr('DRIVER -- not in servo mode!')
return 'FAILURE - not in servo mode'
'''
Send a whole joint trajectory message to a robot...
that is listening to individual joint states.
'''
def send_trajectory(self,traj,acceleration=0.5,velocity=0.5,cartesian=False):
rospy.logerr("Function 'send_trajectory' not implemented for base class!")
return "FAILURE - running base class!"
'''
Send a whole sequence of points to a robot...
that is listening to individual joint states.
'''
def send_sequence(self,traj,acceleration=0.5,velocity=0.5,cartesian=False):
rospy.logerr("Function 'send_sequence' not implemented for base class!")
return "FAILURE - running base class!"
'''
handle changes in driver mode
'''
def handle_mode_tick(self):
rospy.logerr("Function 'handle_mode_tick' not implemented for base class!")
return "FAILURE - running base class!"
'''
Standard movement call.
Tries a cartesian move, then if that fails goes into a joint-space move.
'''
def servo_to_pose_call(self,req):
#rospy.loginfo('Recieved servo to pose request')
#print req
if self.driver_status == 'SERVO':
T = pm.fromMsg(req.target)
# Check acceleration and velocity limits
(acceleration, velocity) = self.check_req_speed_params(req)
# inverse kinematics
traj = self.planner.getCartesianMove(T,self.q0,self.base_steps,self.steps_per_meter)
if len(traj.points) == 0:
(code,res) = self.planner.getPlan(req.target,self.q0) # find a non-local movement
if not res is None:
traj = res.planned_trajectory.joint_trajectory
#if len(traj) == 0:
# traj.append(self.planner.ik(T,self.q0))
# Send command
if len(traj.points) > 0:
rospy.logwarn("Robot moving to " + str(traj.points[-1].positions))
return self.send_trajectory(traj,acceleration,velocity)
else:
rospy.logerr('SIMPLE DRIVER -- IK failed')
return 'FAILURE - not in servo mode'
else:
rospy.logerr('SIMPLE DRIVER -- Not in servo mode')
return 'FAILURE - not in servo mode'
'''
Standard move call.
Make a joint space move to a destination.
'''
def servo_to_joints_call(self,req):
if self.driver_status == 'SERVO':
# Check acceleration and velocity limits
(acceleration, velocity) = self.check_req_speed_params(req)
return 'FAILURE - not yet implemented!'
else:
rospy.logerr('SIMPLE DRIVER -- Not in servo mode')
return 'FAILURE - not in servo mode'
'''
set teach mode
'''
def set_teach_mode_call(self,req):
if req.enable == True:
# self.rob.set_freedrive(True)
self.driver_status = 'TEACH'
return 'SUCCESS - teach mode enabled'
else:
# self.rob.set_freedrive(False)
self.driver_status = 'IDLE'
return 'SUCCESS - teach mode disabled'
'''
send a single point
'''
def send_q(self,pt,acceleration,velocity):
pt = JointTrajectoryPoint()
pt.positions = self.q0
self.pt_publisher.publish(pt)
'''
activate servo mode
'''
def set_servo_mode_call(self,req):
if req.mode == 'SERVO':
self.send_q(self.q0,0.1,0.1)
self.driver_status = 'SERVO'
return 'SUCCESS - servo mode enabled'
elif req.mode == 'DISABLE':
self.driver_status = 'IDLE'
return 'SUCCESS - servo mode disabled'
def shutdown_arm_call(self,req):
self.driver_status = 'SHUTDOWN'
pass
'''
robot-specific logic to update state every "tick"
'''
def handle_tick(self):
rospy.logerr("Function 'handle_tick' not implemented for base class!")
'''
call this when "spinning" to keep updating things
'''
def tick(self):
self.status_publisher.publish(self.driver_status)
self.handle_tick()
def __init__(self, yumi_pb, cfg, exec_thread=True, sim_step_repeat=10):
"""
Class constructor. Sets up internal motion planning interface
for each arm, forward and inverse kinematics solvers, and background
threads for updating the position of the robot.
Args:
yumi_pb (airobot Robot): Instance of PyBullet simulated robot, from
airobot library
cfg (YACS CfgNode): Configuration parameters
exec_thread (bool, optional): Whether or not to start the
background joint position control thread. Defaults to True.
sim_step_repeat (int, optional): Number of simulation steps
to take each time the desired joint position value is
updated. Defaults to 10
"""
self.cfg = cfg
self.yumi_pb = yumi_pb
self.sim_step_repeat = sim_step_repeat
self.joint_lock = threading.RLock()
self._both_pos = self.yumi_pb.arm.get_jpos()
self._single_pos = {}
self._single_pos['right'] = \
self.yumi_pb.arm.arms['right'].get_jpos()
self._single_pos['left'] = \
self.yumi_pb.arm.arms['left'].get_jpos()
self.moveit_robot = moveit_commander.RobotCommander()
self.moveit_scene = moveit_commander.PlanningSceneInterface()
self.moveit_planner = 'RRTConnectkConfigDefault'
self.robot_description = '/robot_description'
self.urdf_string = rospy.get_param(self.robot_description)
self.mp_left = GroupPlanner('left_arm',
self.moveit_robot,
self.moveit_planner,
self.moveit_scene,
max_attempts=3,
planning_time=5.0,
goal_tol=0.5,
eef_delta=0.01,
jump_thresh=10)
self.mp_right = GroupPlanner('right_arm',
self.moveit_robot,
self.moveit_planner,
self.moveit_scene,
max_attempts=3,
planning_time=5.0,
goal_tol=0.5,
eef_delta=0.01,
jump_thresh=10)
self.fk_solver_r = KDLKinematics(URDF.from_parameter_server(),
"yumi_body", "yumi_tip_r")
self.fk_solver_l = KDLKinematics(URDF.from_parameter_server(),
"yumi_body", "yumi_tip_l")
self.num_ik_solver_r = trac_ik.IK('yumi_body',
'yumi_tip_r',
urdf_string=self.urdf_string)
self.num_ik_solver_l = trac_ik.IK('yumi_body',
'yumi_tip_l',
urdf_string=self.urdf_string)
self.ik_pos_tol = 0.001 # 0.001 working well with pulling
self.ik_ori_tol = 0.01 # 0.01 working well with pulling
self.execute_thread = threading.Thread(target=self._execute_both)
self.execute_thread.daemon = True
if exec_thread:
self.execute_thread.start()
self.step_sim_mode = False
else:
self.step_sim_mode = True
def execute_move(self, pos):
# Close the gripper
self._right_gripper.close()
self._right_gripper.close()
self._right_gripper.close()
self._right_gripper.close()
self._right_gripper.close()
rospy.loginfo('moving')
# Get robot parameters and create a limb interface instance
robot = URDF.from_parameter_server()
base_link = robot.get_root()
kdl_kin = KDLKinematics(robot, base_link, 'right_gripper_base')
limb_interface = baxter_interface.limb.Limb('right')
angles = limb_interface.joint_angles()
limb_interface.set_joint_position_speed(.7)
q_goal = [-.01859, -.5119, 1.7909, 1.232, -1.030, 1.945, -1.31]
q0 = kdl_kin.random_joint_angles()
current_angles = [
limb_interface.joint_angle(joint)
for joint in limb_interface.joint_names()
]
for ind in range(len(q0)):
q0[ind] = current_angles[ind]
q_list = JointTrajectory(q0, np.asarray(q_goal), 1, 50, 5)
for q in q_list:
for ind, joint in enumerate(limb_interface.joint_names()):
angles[joint] = q[ind]
limb_interface.set_joint_positions(angles)
rospy.sleep(.03)
# Create the desired joint trajectory with a quintic time scaling
q0 = q_goal
# Set the dropoff position to be dropoff #1
if self._goal == 1:
q_goal = [-.675, -.445, 1.626, 1.1336, -1.457, 1.6145, -2.190]
# Dropoff #2
else:
q_goal = [-.066, -.068, 1.738, .8022, -2.23, .917, -2.9057]
q_list = JointTrajectory(np.asarray(q0), np.asarray(q_goal), 1, 50, 5)
for q in q_list:
for ind, joint in enumerate(limb_interface.joint_names()):
angles[joint] = q[ind]
limb_interface.set_joint_positions(angles)
rospy.sleep(.03)
# Open the gripper
rospy.sleep(.2)
self._right_gripper.open()
self._right_gripper.open()
self._right_gripper.open()
self._right_gripper.open()
self._right_gripper.open()
# Set the position to the old goal and the new goal to the desired camera position for seeing the blocks
q0 = q_goal
q_goal = [-.01859, -.5119, 1.7909, 1.232, -1.030, 1.945, -1.31]
q_list = JointTrajectory(np.asarray(q0), np.asarray(q_goal), 1, 50, 5)
for q in q_list:
for ind, joint in enumerate(limb_interface.joint_names()):
angles[joint] = q[ind]
limb_interface.set_joint_positions(angles)
rospy.sleep(.05)
rospy.sleep(.2)
# Publish next state
self._pub_state.publish(1)
self._done = True
print('Done')
class ClutchNGo_node_handler :
"""
Here is the idea:
While camera clutch is being pressed:
1. Find the vector between two of MTML consecutive positions
2. Use forward kinematics to find ECM current position
3. Add the vector to ECM's current position
4. Use inverse kinematics to find joint angles for new position
"""
class MODE:
"""
simulation mode: Use the hardware for the master side,
and simulation for the ECM
hardware mode: Use hardware for both the master side,
and the ECM
"""
simulation = "SIMULATION"
hardware = "HARDWARE"
def __init__(self):
# For forward and inverse kinematics
self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
self.mtml_robot = URDF.from_parameter_server('/dvrk_mtml/robot_description')
self.mtml_kin = KDLKinematics(self.mtml_robot, self.mtml_robot.links[0].name, self.mtml_robot.links[-1].name)
self.mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description')
self.mtmr_kin = KDLKinematics(self.mtmr_robot, self.mtmr_robot.links[0].name, self.mtmr_robot.links[-1].name)
# For camera clutch control
self.camera_clutch_pressed = False
self.ecm_manual_control_lock_mtml_msg = None
self.ecm_manual_control_lock_ecm_msg = None
self.mtml_start_position = None
self.mtml_end_position = None
self.ecm_msg = None
self.__clutchNGo_mode__ = self.MODE.simulation
self.autocamera = Autocamera()
def __init_nodes__(self):
# Publishers to the simulation
self.ecm_pub = rospy.Publisher('autocamera_node', JointState, queue_size=10)
self.psm1_pub = rospy.Publisher('/dvrk_psm1/joint_states_robot', JointState, queue_size=10)
self.psm2_pub = rospy.Publisher('/dvrk_psm2/joint_states_robot', JointState, queue_size=10)
if self.__clutchNGo_mode__ == self.MODE.simulation:
# Get the ECM joint angles from the simulation
rospy.Subscriber('/dvrk_ecm/joint_states', JointState, self.ecm_cb)
elif self.__clutchNGo_mode__ == self.MODE.hardware:
# Get the ECM joint angles from the hardware
rospy.Subscriber('/dvrk/ECM/state_joint_current', JointState, self.ecm_cb)
# Get the joint angles from MTM hardware
rospy.Subscriber('/dvrk/MTML/state_joint_current', JointState, self.mtml_cb)
#rospy.Subscriber('/dvrk/MTMR/position_joint_current', JointState, add_psm1_jnt)
# Detect whether or not the camera clutch is being pressed
rospy.Subscriber('/dvrk/footpedals/camera', Bool, self.camera_clutch_cb)
def camera_clutch_cb(self, msg):
rospy.logerr('Camera Clutch Pressed : ' + msg.data.__str__())
self.camera_clutch_pressed = msg.data
def mtml_cb(self, msg):
if self.camera_clutch_pressed == True:
if self.current_mtml_pos == None:
self.current_mtml_joint_angles = msg.pos
else:
self.move_ecm( self.current_mtml_joint_angles, msg.pos)
self.current_mtml_joint_angles = msg.pos
else:
self.current_mtml_joint_angles = None
def mtmr_cb(self, msg):
pass
def ecm_cb(self, msg):
self.ecm_msg = msg
pass
def move_ecm(self, first_joint_angles, second_joint_angles):
"""
1. Use forward kinematics to determine the 3d coordinates of the two sets of joint angles
2. Find the vector between those coordinates
3. Use that vector to find a new 3d position for the ECM
4. Use inverse kinematics to find joint angles for the ECM
5. Move the ECM to those joint angles
"""
# (1)
start_coordinates,_ = self.mtml_kin.FK( first_joint_angles[:-1]) # Returns (position, rotation)
end_coordinates,_ = self.mtml_kin.FK(second_joint_angles[:-1])
# (2)
diff = np.subtract(end_coordinates, start_coordinates)
# (3)
ecm_coordinates,_ = self.ecm_kin.FK(self.ecm_msg.position[0:2] + ecm_msg.position[-2:]) # There are a lot of excessive things here that we don't need
ecm_pose = self.ecm_kin.forward(ecm_msg.position[0:2] + ecm_msg.position[-2:])
# Figure out the new orientation and position to be used in the inverse kinematics
b,_ = self.ecm_kin.FK([ecm_msg.position[0],ecm_msg.position[1],.14,ecm_msg.position[3]])
keyhole, _ = self.ecm_kin.FK([0,0,0,0])
ecm_current_direction = b-keyhole
new_ecm_coordinates = np.add(ecm_coordinates, diff)
ecm_new_direction = new_ecm_coordinates - keyhole
r = self.autocamera.find_rotation_matrix_between_two_vectors(ecm_current_direction, ecm_new_direction)
# (4)
ecm_pose[0:3,0:3] = r* ecm_pose[0:3,0:3]
ecm_pose[0:3,3] = new_ecm_coordinates
new_ecm_joint_angles = self.ecm_kin.inverse(ecm_pose)
new_ecm_msg = ecm_msg; new_ecm_msg.position = new_ecm_joint_angles; new_ecm_msg.name = self.ecm_kin.get_joint_names()
self.logerror("ecm_new_direction " + ecm_new_direction.__str__())
self.logerror('ecm_coordinates' + ecm_coordinates.__str__())
self.logerror("ecm_pose " + ecm_pose.__str__())
self.logerror("new_ecm_joint_angles " + new_ecm_joint_angles.__str__())
self.logerror("new_ecm_msg" + new_ecm_msg.__str__())
self.ecm_pub.publish(new_ecm_msg)
pass
def shutdown(self):
rospy.signal_shutdown('shutting down clutchNGo')
def set_mode(self, mode):
""" Values:
MODE.simulation
MODE.hardware
"""
self.__clutchNGo_mode__ = mode
def spin(self):
rospy.spin()
def main():
# Initialize node
rospy.init_node('minimization')
# Start Moveit Commander
InitializeMoveitCommander()
# Initialization of KDL Kinematics for right and left grippers
robot = URDF.from_xml_file(
"/home/josh/catkin_ws/src/baxter_common/baxter_description/urdf/baxter.urdf"
)
global kdl_kin_left, kdl_kin_right
kdl_kin_left = KDLKinematics(robot, "base", "left_gripper")
kdl_kin_right = KDLKinematics(robot, "base", "right_gripper")
# Bounds for SLSQP: s0, s1, e0, e1, w0, w1, w2 (left,right)
bnds = ((-1.70167993878, 1.70167993878), (-2.147, 1.047), (-3.05417993878,
3.05417993878),
(-0.05, 2.618), (-3.059, 3.059), (-1.57079632679, 2.094),
(-3.059, 3.059), (-1.70167993878, 1.70167993878), (-2.147, 1.047),
(-3.05417993878, 3.05417993878), (-0.05, 2.618), (-3.059, 3.059),
(-1.57079632679, 2.094), (-3.059, 3.059)
) # lower and upper bounds for each q (length 14)
# Initial guess
#x0 = np.full((14,1), 0.75)
#x0 = np.array([[random.uniform(bnds[i][0]/2.,bnds[i][1]/2.)] for i in range(14)])
initial_left = Pose()
initial_left.position = Point(
x=0.3, #(P_left_current[0,0] + P_right_current[0,0])/2.,
y=(P_left_current[1, 0] + P_right_current[1, 0]) / 2.,
z=(P_left_current[2, 0] + P_right_current[2, 0]) / 2.,
)
initial_left.orientation = Quaternion(
x=0.0,
y=0.0,
z=0.0,
w=1.0,
)
initial_right = Pose()
initial_right.position = Point(
x=0.3, #(P_left_current[0,0] + P_right_current[0,0])/2.,
y=(P_left_current[1, 0] + P_right_current[1, 0]) / 2.,
z=(P_left_current[2, 0] + P_right_current[2, 0]) / 2.,
)
initial_right.orientation = Quaternion(
x=0.0,
y=0.0,
z=0.0,
w=1.0,
)
x0_left = kdl_kin_left.inverse(initial_left)
x0_right = kdl_kin_left.inverse(initial_right)
x0 = np.array([[x0_left[0]], [x0_left[1]], [x0_left[2]], [x0_left[3]],
[x0_left[4]], [x0_left[5]], [x0_left[6]], [x0_right[0]],
[x0_right[1]], [x0_right[2]], [x0_right[3]], [x0_right[4]],
[x0_right[5]], [x0_right[6]]])
x0 = x0 + random.uniform(-0.2, 0.2)
# Constraint equality
Handoff_separation = np.array([[0.0], [0.1], [0.0], [math.pi / 1.], [0],
[-math.pi / 2.]])
cons = (
{
'type': 'eq',
'fun': lambda q: JS_to_PrPlRrl(q)[9, 0] - Handoff_separation[0, 0]
}, #x-sep-distance = 0
{
'type': 'eq',
'fun': lambda q: JS_to_PrPlRrl(q)[10, 0] - Handoff_separation[2, 0]
}, #y-sep-distance = 0
{
'type': 'eq',
'fun': lambda q: JS_to_PrPlRrl(q)[11, 0] - Handoff_separation[1, 0]
}, #z-sep-distance = 0.1
{
'type':
'eq',
'fun':
lambda q: math.fabs(JS_to_PrPlRrl(q)[6, 0]) - Handoff_separation[3,
0]
}, #roll = pi
{
'type':
'eq',
'fun':
lambda q: math.fabs(JS_to_PrPlRrl(q)[7, 0]) - Handoff_separation[4,
0]
}, #pitch = 0
{
'type': 'eq',
'fun': lambda q: JS_to_PrPlRrl(q)[8, 0] - Handoff_separation[5, 0]
}, #yaw = -pi/2
#{'type': 'ineq', 'fun': lambda q: JS_to_PrPlRrl(q)[0,0]-0.3}, #x-pos > 0.3 m to help avoid collisions
{
'type':
'eq',
'fun':
lambda q: JS_to_PrPlRrl(q)[0, 0] -
(P_left_current[0, 0] + P_right_current[0, 0]) / 2.
},
{
'type':
'eq',
'fun':
lambda q: JS_to_PrPlRrl(q)[1, 0] -
(P_left_current[1, 0] + P_right_current[1, 0]) / 2.
})
#{'type': 'eq', 'fun': lambda q: JS_to_PrPlRrl(q)[0,0] - (P_left_current[0,0] + P_right_current[0,0])/2.})
# Minimization
before = rospy.get_rostime()
result = minimize(minimization,
x0,
method='SLSQP',
jac=jacobian,
bounds=bnds,
constraints=cons,
tol=0.1,
options={'maxiter': 100})
after = rospy.get_rostime()
print "\nNumber of iterations: \n", result.success, result.nit
print result
q_left = result.x[0:7]
q_right = result.x[7:14]
pose_left = kdl_kin_left.forward(q_left)
pose_right = kdl_kin_right.forward(q_right)
print "\nPose left: \n", pose_left
print "Pose right: \n", pose_right
#R_left = pose_left[:3,:3]
#R_right = pose_right[:3,:3]
#X_left,Y_left,Z_left = euler_from_matrix(R_left, 'sxyz')
#X_right,Y_right,Z_right = euler_from_matrix(R_right, 'sxyz')
q = np.array([
q_left[0], q_left[1], q_left[2], q_left[3], q_left[4], q_left[5],
q_left[6], q_right[0], q_right[1], q_right[2], q_right[3], q_right[4],
q_right[5], q_right[6]
])
#P = JS_to_PrPlRrl(q)
handoff_distance = math.sqrt((pose_left[0, 3] - pose_right[0, 3])**2 +
(pose_left[1, 3] - pose_right[1, 3])**2 +
(pose_left[2, 3] - pose_right[2, 3])**2)
print "\nDistance between handoff: ", handoff_distance
print "Minimization time: ", (
after.secs - before.secs) + (after.nsecs - before.nsecs) / 1e+9
if result.success == False or math.fabs(handoff_distance - 0.1) > 0.05:
print "Minimization was a failure."
elif result.success == True:
print "Minimization was a success."
print "Planning a trajectory in MoveIt!"
#s0, s1, e0, e1, w0, w1, w2
joints = {
'left_s0': q_left[0],
'left_s1': q_left[1],
'left_e0': q_left[2],
'left_e1': q_left[3],
'left_w0': q_left[4],
'left_w1': q_left[5],
'left_w2': q_left[6],
'right_s0': q_right[0],
'right_s1': q_right[1],
'right_e0': q_right[2],
'right_e1': q_right[3],
'right_w0': q_right[4],
'right_w1': q_right[5],
'right_w2': q_right[6]
}
group_both_arms.set_joint_value_target(joints)
plan_both = group_both_arms.plan()
print "Trajectory time (nsec): ", plan_both.joint_trajectory.points[
len(plan_both.joint_trajectory.points) - 1].time_from_start
class Autocamera_node_handler:
# move the actual ecm with sliders?
__MOVE_ECM_WITH_SLIDERS__ = False
class MODE:
simulation = "SIMULATION"
hardware = "HARDWARE"
sliders = "SLIDERS"
DEBUG = True
def __init__(self):
self.t = time.time()
self.__AUTOCAMERA_MODE__ = self.MODE.simulation
self.autocamera = Autocamera() # Instantiate the Autocamera Class
self.jnt_msg = JointState()
self.joint_angles = {'ecm':None, 'psm1':None, 'psm2':None}
self.cam_info = {'left':CameraInfo(), 'right':CameraInfo()}
self.last_ecm_jnt_pos = None
self.first_run = True
# For forward and inverse kinematics
self.ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
self.ecm_kin = KDLKinematics(self.ecm_robot, self.ecm_robot.links[0].name, self.ecm_robot.links[-1].name)
self.psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
self.psm1_kin = KDLKinematics(self.psm1_robot, self.psm1_robot.links[0].name, self.psm1_robot.links[-1].name)
self.mtml_robot = URDF.from_parameter_server('/dvrk_mtml/robot_description')
self.mtml_kin = KDLKinematics(self.mtml_robot, self.mtml_robot.links[0].name, self.mtml_robot.links[-1].name)
self.mtmr_robot = URDF.from_parameter_server('/dvrk_mtmr/robot_description')
self.mtmr_kin = KDLKinematics(self.mtmr_robot, self.mtmr_robot.links[0].name, self.mtmr_robot.links[-1].name)
# For camera clutch control
self.camera_clutch_pressed = False
self.ecm_manual_control_lock_mtml_msg = None
self.ecm_manual_control_lock_ecm_msg = None
self.mtml_start_position = None
self.mtml_end_position = None
self.initialize_psms_initialized = 30
self.__DEBUG_GRAPHICS__ = False
def __init_nodes__(self):
self.ecm_hw = robot('ECM')
self.psm1_hw = robot('PSM1')
self.psm2_hw = robot('PSM2')
#rospy.init_node('autocamera_node')
self.logerror("start", debug=True)
# Publishers to the simulation
self.ecm_pub = rospy.Publisher('/dvrk_ecm/joint_states_robot', JointState, queue_size=10)
self.psm1_pub = rospy.Publisher('/dvrk_psm1/joint_states_robot', JointState, queue_size=10)
self.psm2_pub = rospy.Publisher('/dvrk_psm2/joint_states_robot', JointState, queue_size=10)
# Get the joint angles from the simulation
rospy.Subscriber('/dvrk_ecm/joint_states', JointState, self.add_ecm_jnt)
rospy.Subscriber('/fakecam_node/camera_info', CameraInfo, self.get_cam_info)
try:
self.sub_psm1_sim.unregister()
self.sub_psm2_sim.unregister()
self.sub_psm1_hw.unregister()
self.sub_psm2_hw.unregister()
except Exception:
pass
if self.__AUTOCAMERA_MODE__ == self.MODE.hardware :
# Get the joint angles from the hardware and move the simulation from hardware
self.sub_psm1_hw = rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, self.add_psm1_jnt)
self.sub_psm2_hw = rospy.Subscriber('/dvrk/PSM2/state_joint_current', JointState, self.add_psm2_jnt)
elif self.__AUTOCAMERA_MODE__ == self.MODE.simulation:
# Get the joint angles from the simulation
self.sub_psm1_sim = rospy.Subscriber('/dvrk_psm1/joint_states', JointState, self.add_psm1_jnt)
self.sub_psm2_sim = rospy.Subscriber('/dvrk_psm2/joint_states', JointState, self.add_psm2_jnt)
# If hardware is connected, subscribe to it and set the psm joint angles in the simulation from the hardware
self.sub_psm1_hw = rospy.Subscriber('/dvrk/PSM1/state_joint_current', JointState, self.add_psm1_jnt_from_hw)
self.sub_psm2_hw = rospy.Subscriber('/dvrk/PSM2/state_joint_current', JointState, self.add_psm2_jnt_from_hw)
# Get the joint angles from MTM hardware
##rospy.Subscriber('/dvrk/MTML/position_joint_current', JointState, self.mtml_cb)
# rospy.Subscriber('/dvrk/MTMR/position_joint_current', JointState, add_psm1_jnt)
# Detect whether or not the camera clutch is being pressed
## rospy.Subscriber('/dvrk/footpedals/camera', Bool, self.camera_clutch_cb)
# Move the hardware from the simulation
# rospy.Subscriber('/dvrk_psm1/joint_states', JointState, self.move_psm1)
# rospy.Subscriber('/dvrk_psm2/joint_states', JointState, self.move_psm2)
if self.__DEBUG_GRAPHICS__ == True:
# Subscribe to fakecam images
rospy.Subscriber('/fakecam_node/fake_image_left', Image, self.left_image_cb)
rospy.Subscriber('/fakecam_node/fake_image_right', Image, self.right_image_cb)
# Publish images
self.image_left_pub = rospy.Publisher('autocamera_image_left', Image, queue_size=10)
self.image_right_pub = rospy.Publisher('autocamera_image_right', Image, queue_size=10)
def shutdown(self):
rospy.signal_shutdown('shutting down Autocamera')
# This needs to be run before anything can be expected
def spin(self):
self.__init_nodes__() # initialize all the nodes, subscribers and publishers
rospy.spin()
def debug_graphics(self, has_graphics):
self.__DEBUG_GRAPHICS__ = has_graphics
def logerror(self, msg, debug = False):
if self.DEBUG or debug:
rospy.logerr(msg)
def ecm_manual_control_lock(self, msg, fun):
if fun == 'ecm':
self.ecm_manual_control_lock_ecm_msg = msg
elif fun == 'mtml':
self.ecm_manual_control_lock_mtml_msg = msg
if self.ecm_manual_control_lock_mtml_msg != None and self.ecm_manual_control_lock_ecm_msg != None:
self.ecm_manual_control(self.ecm_manual_control_lock_mtml_msg, self.ecm_manual_control_lock_ecm_msg)
self.ecm_manual_control_lock_mtml_msg = None
self.ecm_manual_control_lock_ecm_msg = None
def ecm_manual_control(self, mtml_msg, ecm_msg):
# TODO: Find forward kinematics from mtmr and ecm. Move mtmr. Find the movement vector. Add it to the
# ecm position, use inverse kinematics and move the ecm.
self.logerror("mtml" + self.mtml_kin.get_joint_names().__str__())
start_coordinates,_ = self.mtml_kin.FK( self.mtml_start_position.position[:-1]) # Returns (position, rotation)
end_coordinates,_ = self.mtml_kin.FK(mtml_msg.position[:-1])
diff = np.subtract(end_coordinates, start_coordinates)
self.logerror("diff = " + diff.__str__())
# Find the ecm 3d coordinates, add the 'diff' to it, then do an inverse kinematics
ecm_coordinates,_ = self.ecm_kin.FK(ecm_msg.position[0:2] + ecm_msg.position[-2:]) # There are a lot of excessive things here that we don't need
ecm_pose = self.ecm_kin.forward(ecm_msg.position[0:2] + ecm_msg.position[-2:])
# Figure out the new orientation and position to be used in the inverse kinematics
b,_ = self.ecm_kin.FK([ecm_msg.position[0],ecm_msg.position[1],.14,ecm_msg.position[3]])
keyhole, _ = self.ecm_kin.FK([0,0,0,0])
ecm_current_direction = b-keyhole
new_ecm_coordinates = np.add(ecm_coordinates, diff)
ecm_new_direction = new_ecm_coordinates - keyhole
r = self.autocamera.find_rotation_matrix_between_two_vectors(ecm_current_direction, ecm_new_direction)
ecm_pose[0:3,0:3] = r* ecm_pose[0:3,0:3]
ecm_pose[0:3,3] = new_ecm_coordinates
new_ecm_joint_angles = self.ecm_kin.inverse(ecm_pose)
new_ecm_msg = ecm_msg; new_ecm_msg.position = new_ecm_joint_angles; new_ecm_msg.name = self.ecm_kin.get_joint_names()
self.logerror("ecm_new_direction " + ecm_new_direction.__str__())
self.logerror('ecm_coordinates' + ecm_coordinates.__str__())
self.logerror("ecm_pose " + ecm_pose.__str__())
self.logerror("new_ecm_joint_angles " + new_ecm_joint_angles.__str__())
self.logerror("new_ecm_msg" + new_ecm_msg.__str__())
self.ecm_pub.publish(new_ecm_msg)
def camera_clutch_cb(self, msg):
self.camera_clutch_pressed = msg.data
self.logerror('Camera Clutch : ' + self.camera_clutch_pressed.__str__())
def mtml_cb(self, msg):
if self.camera_clutch_pressed :
if self.mtml_start_position == None:
self.mtml_start_position = msg
self.mtml_end_position = msg
#Freeze mtms
### CODE HERE ###
# move the ecm based on the movement of MTML
self.ecm_manual_control_lock(msg, 'mtml')
else:
self.mtml_start_position = msg
self.mtml_end_position = None
def move_psm1(self, msg):
jaw = msg.position[-3]
msg = self.autocamera.extract_positions(msg, 'psm1')
msg.name = msg.name + ['jaw']
msg.position = msg.position + [jaw]
self.psm1_hw.move_joint_list(msg.position, interpolate=True)
def move_psm2(self, msg):
time.sleep(.5)
jaw = msg.position[-3]
msg = self.autocamera.extract_positions(msg, 'psm2')
msg.name = msg.name + ['jaw']
msg.position = msg.position + [jaw]
self.psm2_hw.move_joint_list(msg.position, interpolate=first_run)
# ecm callback
def add_ecm_jnt(self, msg):
if self.camera_clutch_pressed == False and msg != None:
if self.__MOVE_ECM_WITH_SLIDERS__ == False:
self.add_jnt('ecm', msg)
else:
temp = list(msg.position[:2]+msg.position[-2:])
r = self.ecm_hw.move_joint_list(temp, interpolate=first_run)
if r == True:
self.first_run = False
else:
self.ecm_manual_control_lock(msg, 'ecm')
def add_psm1_jnt_from_hw(self, msg):
if self.camera_clutch_pressed == False and msg != None:
if self.__AUTOCAMERA_MODE__ == self.MODE.simulation:
msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw']
self.psm1_pub.publish(msg)
def add_psm2_jnt_from_hw(self, msg):
if self.camera_clutch_pressed == False and msg != None:
if self.__AUTOCAMERA_MODE__ == self.MODE.simulation:
msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw']
self.psm2_pub.publish(msg)
# psm1 callback
def add_psm1_jnt(self, msg):
if self.camera_clutch_pressed == False and msg != None:
# We need to set the names, otherwise the simulation won't move
if self.__AUTOCAMERA_MODE__ == self.MODE.hardware:
msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw']
self.psm1_pub.publish(msg)
self.add_jnt('psm1', msg)
# psm2 callback
def add_psm2_jnt(self, msg):
if self.camera_clutch_pressed == False and msg != None:
if self.__AUTOCAMERA_MODE__ == self.MODE.hardware :
msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw']
self.psm2_pub.publish(msg)
self.add_jnt('psm2', msg)
def add_jnt(self, name, msg):
self.joint_angles[name] = msg
if not None in self.joint_angles.values():
if self.initialize_psms_initialized>0 and self.__AUTOCAMERA_MODE__ == self.MODE.simulation:
self.initialize_psms()
time.sleep(.01)
self.initialize_psms_initialized -= 1
try:
jnt_msg = 'error'
jnt_msg = self.autocamera.compute_viewangle(self.joint_angles, self.cam_info)
self.ecm_pub.publish(jnt_msg)
jnt_msg.position = [ round(i,4) for i in jnt_msg.position]
if len(jnt_msg.position) != 4 or len(jnt_msg.name) != 4 :
return
#return # stop here until we co-register the arms
if self.__AUTOCAMERA_MODE__ == self.MODE.hardware:
pos = jnt_msg.position
result = self.ecm_hw.move_joint_list(pos, index=[0,1,2,3], interpolate=self.first_run)
# Interpolate the insertion joint individually and the rest without interpolation
# pos = [jnt_msg.position[2]]
# result = self.ecm_hw.move_joint_list(pos, index=[2], interpolate=True)
if result:
self.first_run = False
#
#
except TypeError:
# rospy.logerr('Exception : ' + TypeError.message.__str__())
pass
self.joint_angles = dict.fromkeys(self.joint_angles, None)
# camera info callback
def get_cam_info(self, msg):
if msg.header.frame_id == '/fake_cam_left_optical_link':
self.cam_info['left'] = msg
elif msg.header.frame_id == '/fake_cam_right_optical_link':
self.cam_info['right'] = msg
def image_cb(self, image_msg, camera_name):
image_pub = {'left':self.image_left_pub, 'right':self.image_right_pub}[camera_name]
bridge = cv_bridge.CvBridge()
im = bridge.imgmsg_to_cv2(image_msg, 'rgb8')
tool1_name = {'left':'l1', 'right':'r1'}
tool2_name = {'left':'l2', 'right':'r2'}
toolm_name = {'left':'lm', 'right':'rm'}
tool1 = self.autocamera.zoom_level_positions[tool1_name[camera_name]]; tool1 = tuple(int(i) for i in tool1)
tool2 = self.autocamera.zoom_level_positions[tool2_name[camera_name]]; tool2 = tuple(int(i) for i in tool2)
toolm = self.autocamera.zoom_level_positions[toolm_name[camera_name]]; toolm = tuple(int(i) for i in toolm)
rotate_180 = lambda p : (640-p[0], 480-p[2])
cv2.circle(im, tool1, 10, (0,255,0), -1)
cv2.circle(im, tool2, 10, (0,255,255), -1)
cv2.circle(im, toolm, 10, (0,0,255), -1)
cv2.circle(im, (0,0), 20, (255,0,0), -1)
cv2.circle(im, (640,480), 20, (255,0,255), -1)
new_image = bridge.cv2_to_imgmsg(im, 'rgb8')
new_image.header.seq = image_msg.header.seq
new_image.header.stamp = image_msg.header.stamp
new_image.header.frame_id = image_msg.header.frame_id
image_pub.publish(new_image)
def left_image_cb(self, image_msg):
self.image_cb(image_msg, 'left')
def right_image_cb(self, image_msg):
self.image_cb(image_msg, 'right')
def initialize_psms(self):
if self.__AUTOCAMERA_MODE__ == self.MODE.simulation :
msg = JointState()
msg.name = ['outer_yaw', 'outer_pitch', 'outer_insertion', 'outer_roll', 'outer_wrist_pitch', 'outer_wrist_yaw', 'jaw']
msg.position = [0.84 , -0.65, 0.10, 0.00, 0.00, 0.00, 0.00]
self.psm1_pub.publish(msg)
msg.position = [-0.84 , -0.53, 0.10, 0.00, 0.00, 0.00, 0.00]
self.psm2_pub.publish(msg)
self.logerror('psms initialized!')
def set_mode(self, mode):
""" Values:
MODE.simulation
MODE.hardware
"""
self.__AUTOCAMERA_MODE__ = mode
class SimplePlanning:
def __init__(self,
robot,
base_link,
end_link,
group,
move_group_ns="move_group",
planning_scene_topic="planning_scene",
robot_ns="",
verbose=False,
kdl_kin=None,
closed_form_IK_solver=None,
joint_names=[]):
self.robot = robot
self.tree = kdl_tree_from_urdf_model(self.robot)
self.chain = self.tree.getChain(base_link, end_link)
if kdl_kin is None:
self.kdl_kin = KDLKinematics(self.robot, base_link, end_link)
else:
self.kdl_kin = kdl_kin
self.base_link = base_link
self.joint_names = joint_names
self.end_link = end_link
self.group = group
self.robot_ns = robot_ns
self.client = actionlib.SimpleActionClient(move_group_ns,
MoveGroupAction)
self.verbose = verbose
self.closed_form_IK_solver = closed_form_IK_solver
'''
ik: handles calls to KDL inverse kinematics
'''
def ik(self, T, q0, dist=0.5):
q = None
if self.closed_form_IK_solver is not None:
#T = pm.toMatrix(F)
q = self.closed_form_IK_solver.findClosestIK(T, q0)
else:
q = self.kdl_kin.inverse(T, q0)
# NOTE: this results in unsafe behavior; do not use without checks
#if q is None:
# q = self.kdl_kin.inverse(T)
return q
'''
TODO: finish this
'''
def getCartesianMove(self,
frame,
q0,
base_steps=1000,
steps_per_meter=1000,
time_multiplier=10):
# interpolate between start and goal
pose = pm.fromMatrix(self.kdl_kin.forward(q0))
cur_rpy = np.array(pose.M.GetRPY())
cur_xyz = np.array(pose.p)
goal_rpy = np.array(frame.M.GetRPY())
goal_xyz = np.array(frame.p)
steps = base_steps + int((pose.p - frame.p).Norm() * steps_per_meter)
print " -- Computing %f steps" % steps
ts = (pose.p - frame.p).Norm() / steps * time_multiplier
traj = JointTrajectory()
traj.points.append(
JointTrajectoryPoint(positions=q0,
velocities=[0] * len(q0),
accelerations=[0] * len(q0)))
# compute IK
for i in range(1, steps + 1):
xyz = cur_xyz + ((float(i) / steps) * (goal_xyz - cur_xyz))
rpy = cur_rpy + ((float(i) / steps) * (goal_rpy - cur_rpy))
frame = pm.toMatrix(
kdl.Frame(kdl.Rotation.RPY(rpy[0], rpy[1], rpy[2]),
kdl.Vector(xyz[0], xyz[1], xyz[2])))
#q = self.kdl_kin.inverse(frame,q0)
q = self.ik(frame, q0)
if self.verbose:
print "%d -- %s %s = %s" % (i, str(xyz), str(rpy), str(q))
if not q is None:
pt = JointTrajectoryPoint(positions=q,
velocities=[0] * len(q),
accelerations=[0] * len(q))
pt.time_from_start = rospy.Duration(i * ts)
traj.points.append(pt)
q0 = q
elif i == steps:
return JointTrajectory()
if len(traj.points) < base_steps:
print rospy.logerr("Planning failure with " \
+ str(len(traj.points)) \
+ " / " + str(base_steps) \
+ " points.")
return JointTrajectory()
traj.joint_names = self.joint_names
return traj
def getGoalConstraints(self, frames, q, timeout=2.0, mode=ModeJoints):
is_list_of_frame = isinstance(frames, list)
number_of_frames = None
if len(self.robot_ns) > 0:
srv = rospy.ServiceProxy(self.robot_ns + "/compute_ik",
moveit_msgs.srv.GetPositionIK)
else:
srv = rospy.ServiceProxy("compute_ik",
moveit_msgs.srv.GetPositionIK)
goal = Constraints()
if is_list_of_frame:
number_of_frames = len(frames)
else:
number_of_frames = 1
# print "Start looping thru cartesian points"
for i in xrange(number_of_frames):
frame = None
joints = None
previous_q = None
if is_list_of_frame:
print "is a list of frames"
frame = frames[i]
else:
"just one frame"
frame = frames
if i == 0:
previous_q = q
joints = self.ik(pm.toMatrix(frame), previous_q)
if joints is not None:
previous_q = joints
else:
return (None, None)
# print joints is None
# print joints
# if joints is None:
# p = geometry_msgs.msg.PoseStamped()
# p.pose.position.x = frame.position.x
# p.pose.position.y = frame.position.y
# p.pose.position.z = frame.position.z
# p.pose.orientation.x = frame.orientation.x
# p.pose.orientation.y = frame.orientation.y
# p.pose.orientation.z = frame.orientation.z
# p.pose.orientation.w = frame.orientation.w
# p.header.frame_id = "/world"
# ik_req = moveit_msgs.msg.PositionIKRequest()
# ik_req.robot_state.joint_state.name = self.joint_names
# ik_req.robot_state.joint_state.position = q
# ik_req.avoid_collisions = True
# ik_req.timeout = rospy.Duration(timeout)
# ik_req.attempts = 5
# ik_req.group_name = self.group
# ik_req.pose_stamped = p
# rospy.logwarn("Getting IK position...")
# ik_resp = srv(ik_req)
# rospy.logwarn("IK RESULT ERROR CODE = %d"%(ik_resp.error_code.val))
# #if ik_resp.error_code.val > 0:
# # return (ik_resp, None)
# #print ik_resp.solution
# ###############################
# # now create the goal based on inverse kinematics
# if not ik_resp.error_code.val < 0:
# for i in range(0,len(ik_resp.solution.joint_state.name)):
# print ik_resp.solution.joint_state.name[i]
# print ik_resp.solution.joint_state.position[i]
# joint = JointConstraint()
# joint.joint_name = ik_resp.solution.joint_state.name[i]
# joint.position = ik_resp.solution.joint_state.position[i]
# joint.tolerance_below = 0.005
# joint.tolerance_above = 0.005
# joint.weight = 1.0
# goal.joint_constraints.append(joint)
# return (ik_resp, goal)
if mode == ModeJoints:
for i in range(0, len(self.joint_names)):
joint = JointConstraint()
joint.joint_name = self.joint_names[i]
joint.position = joints[i]
joint.tolerance_below = 0.005
joint.tolerance_above = 0.005
joint.weight = 1.0
goal.joint_constraints.append(joint)
else:
print 'Cartesian Constraint', self.base_link, self.end_link
# TODO: Try to fix this again. Something is wrong
position_costraint = PositionConstraint()
position_costraint.link_name = self.end_link
position_costraint.target_point_offset.x = 0.0
position_costraint.target_point_offset.y = 0.0
position_costraint.target_point_offset.z = 0.0
sphere_bounding = SolidPrimitive()
sphere_bounding.type = sphere_bounding.SPHERE
sphere_bounding.dimensions.append(0.005)
position_costraint.constraint_region.primitives.append(
sphere_bounding)
sphere_pose = Pose()
sphere_pose.position = frame.p
sphere_pose.orientation.x = 0.0
sphere_pose.orientation.y = 0.0
sphere_pose.orientation.z = 0.0
sphere_pose.orientation.w = 1.0
position_costraint.constraint_region.primitive_poses.append(
sphere_pose)
position_costraint.weight = 1.0
position_costraint.header.frame_id = '/world'
goal.position_constraints.append(position_costraint)
orientation_costraint = OrientationConstraint()
orientation_costraint.link_name = self.end_link
orientation_costraint.header.frame_id = '/world'
orientation_costraint.orientation = frame.M.GetQuaternion()
orientation_costraint.absolute_x_axis_tolerance = 0.005
orientation_costraint.absolute_y_axis_tolerance = 0.005
orientation_costraint.absolute_z_axis_tolerance = 0.005
orientation_costraint.weight = 1.0
goal.orientation_constraints.append(orientation_costraint)
return (None, goal)
def updateAllowedCollisions(self, obj, allowed):
self.planning_scene_publisher = rospy.Publisher(
'planning_scene', PlanningScene)
rospy.wait_for_service('/get_planning_scene', 10.0)
get_planning_scene = rospy.ServiceProxy('/get_planning_scene',
GetPlanningScene)
request = PlanningSceneComponents(
components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
if not obj in acm.default_entry_names:
# add button to allowed collision matrix
acm.default_entry_names += [obj]
acm.default_entry_values += [allowed]
else:
idx = acm.default_entry_names.index(obj)
acm.default_entry_values[idx] = allowed
planning_scene_diff = PlanningScene(is_diff=True,
allowed_collision_matrix=acm)
self.planning_scene_publisher.publish(planning_scene_diff)
def getPlan(self, frame, q, obj=None, compute_ik=True):
planning_options = PlanningOptions()
planning_options.plan_only = True
planning_options.replan = False
planning_options.replan_attempts = 0
planning_options.replan_delay = 0.1
planning_options.planning_scene_diff.is_diff = True
planning_options.planning_scene_diff.robot_state.is_diff = True
if obj is not None:
self.updateAllowedCollisions(obj, True)
motion_req = MotionPlanRequest()
motion_req.start_state.joint_state.position = q
motion_req.workspace_parameters.header.frame_id = self.base_link
motion_req.workspace_parameters.max_corner.x = 1.0
motion_req.workspace_parameters.max_corner.y = 1.0
motion_req.workspace_parameters.max_corner.z = 1.0
motion_req.workspace_parameters.min_corner.x = -1.0
motion_req.workspace_parameters.min_corner.y = -1.0
motion_req.workspace_parameters.min_corner.z = -1.0
# create the goal constraints
# TODO: change this to use cart goal(s)
# - frame: take a list of frames
# - returns: goal contraints
if compute_ik:
(ik_resp, goal) = self.getGoalConstraints(frame,
q,
mode=ModeJoints)
#if (ik_resp.error_code.val > 0):
# return (1,None)
print 'IK error code: ', ik_resp
# print goal
motion_req.goal_constraints.append(goal)
motion_req.group_name = self.group
motion_req.num_planning_attempts = 10
motion_req.allowed_planning_time = 4.0
motion_req.planner_id = "RRTConnectkConfigDefault"
if goal is None or len(
motion_req.goal_constraints[0].joint_constraints) == 0 or (
not ik_resp is None and ik_resp.error_code.val < 0):
return (-31, None)
goal = MoveGroupGoal()
goal.planning_options = planning_options
goal.request = motion_req
rospy.logwarn("Sending request...")
self.client.send_goal(goal)
self.client.wait_for_result()
res = self.client.get_result()
rospy.logwarn("Done: " + str(res.error_code.val))
if obj is not None:
self.updateAllowedCollisions(obj, False)
return (res.error_code.val, res)
def main():
rospy.init_node('set_base_frames')
sleep (1)
global psm1_kin, psm1_robot, psm2_kin, psm2_robot, ecm_kin, ecm_robot
if psm1_robot is None:
psm1_robot = URDF.from_parameter_server('/dvrk_psm1/robot_description')
psm1_kin = KDLKinematics(psm1_robot, psm1_robot.links[0].name, psm1_robot.links[1].name)
if psm2_robot is None:
psm2_robot = URDF.from_parameter_server('/dvrk_psm2/robot_description')
psm2_kin = KDLKinematics(psm2_robot, psm2_robot.links[0].name, psm2_robot.links[1].name)
if ecm_robot is None:
ecm_robot = URDF.from_parameter_server('/dvrk_ecm/robot_description')
ecm_kin = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[-1].name)
ecm_base = KDLKinematics(ecm_robot, ecm_robot.links[0].name, ecm_robot.links[3].name)
# pdb.set_trace()
mtml_pub = rospy.Publisher('/dvrk/MTML/set_base_frame', Pose, queue_size=1)
mtmr_pub = rospy.Publisher('/dvrk/MTMR/set_base_frame', Pose, queue_size=1)
ecm_pub = rospy.Publisher('/dvrk/ECM/set_base_frame', Pose, queue_size=1)
psm1_pub = rospy.Publisher('/dvrk/PSM1/set_base_frame', Pose, queue_size=1)
psm2_pub = rospy.Publisher('/dvrk/PSM2/set_base_frame', Pose, queue_size=1)
mtmr_psm1 = rospy.Publisher('/dvrk/MTMR_PSM1/set_registration_rotation', Quaternion, queue_size=1)
mtml_psm2 = rospy.Publisher('/dvrk/MTML_PSM2/set_registration_rotation', Quaternion, queue_size=1)
p1_base = psm1_kin.forward([]) # PSM1 Base Frame
p2_base = psm2_kin.forward([]) # PSM2 Base Frame
e_base = ecm_base.forward([]) # ECM Base Frame
e = ecm_kin.forward([0,0,0,0]) # ECM Tool Tip
camera_view_transform = pose_converter.PoseConv.to_homo_mat( [(0.0,0.0,0.0), (1.57079632679, 0.0, 0.0)])
r = lambda axis, rad: rotate(axis, rad)
mtmr_m = e;# mtmr_m = mtmr_m**-1
mtml_m = e
print 'qmat'
qmsg = Quaternion()
temp = quat_from_homomat(p1_base)
print quat_from_homomat(p1_base)
message = pose_from_homomat(p1_base);
psm1_pub.publish(message)
while not rospy.is_shutdown():
#print p1_base
#print message
psm1_pub.publish(message)
print ('sure\n')
# psm2_pub.publish(pose_from_homomat(p2_base))
# psm1_pub.publish(pose_from_homomat(e_base))
# mtml_pub.publish( pose_from_homomat(mtml_m))
# mtmr_pub.publish( pose_from_homomat(mtmr_m))
print ('\n\Hello: nmtml rotation: \n')
print( mtml_m.__repr__( ))
print(pose_from_homomat(mtml_m))
print ('\n\nmtmr rotation: \n')
print( mtmr_m.__repr__())
class ReachingGame:
def __init__(self, robot):
self.reaching_bound_pos = 0.03
self.reaching_bound_rot = 0.3
self.reaching_target = [
[0.5, 0.05, 0.8, -1.0, 0.0, 0.0, 6.123233995736766e-17],
[0.5, 0, 0.9, -1.0, 0.0, 0.0, 6.123233995736766e-17],
[0.45, 0.15, 0.9, -1.0, 0.0, 0.0, 6.123233995736766e-17],
[
0.4, 0.1, 0.9, -0.9659258262890683, 1.5848e-17, -0.2588,
5.91e-17
],
[
0.4, 0.1, 0.9, -0.8660254037844387, 3.0616e-17, -0.4999,
5.30e-17
],
[
0.45, 0.15, 0.9, -0.7500000000000001, -0.24999999999999986,
-0.4330127018922193, -0.4330127018922192
]
]
self.num_target = len(self.reaching_target)
self.CubeState = LinkState()
self.CubeState.link_name = "jaco_on_table::Cube3"
self.CubeState.reference_frame = "jaco_on_table::robot_base"
cube_topic = "/gazebo/set_link_state"
self.pub_cube = rospy.Publisher(cube_topic,
gazebo_msgs.msg.LinkState,
queue_size=1)
self.kdl_kin = KDLKinematics(robot, "robot_base",
"jaco_fingers_base_link")
self.current_target = 0
self.end_game = False
self.time_taken = [0] * self.num_target
self.current_time = time.time()
def game_start(self):
self.set_target(self.current_target)
def set_target(self, i):
self.CubeState.pose.position.x = self.reaching_target[i][0]
self.CubeState.pose.position.y = self.reaching_target[i][1]
self.CubeState.pose.position.z = self.reaching_target[i][2]
self.CubeState.pose.orientation.x = self.reaching_target[i][3]
self.CubeState.pose.orientation.y = self.reaching_target[i][4]
self.CubeState.pose.orientation.z = self.reaching_target[i][5]
self.CubeState.pose.orientation.w = self.reaching_target[i][6]
self.pub_cube.publish(self.CubeState)
print "Target set to ", self.current_target
def test_reached(self, currentJointState):
self.pub_cube.publish(self.CubeState)
# test if the current position is reached within a bound
cur_pose = self.kdl_kin.forward(currentJointState)
cur_q = RotationMatrix2Quaternion(cur_pose[0:3, 0:3])
current = [
cur_pose[0, 3].item(0), cur_pose[1, 3].item(0),
cur_pose[2, 3].item(0), cur_q[0], cur_q[1], cur_q[2], cur_q[3]
]
target = self.reaching_target[self.current_target]
# print target[0] - current[0], target[1] - current[1], target[2] - current[2]
for i in range(3):
if np.abs(target[i] - current[i]) > self.reaching_bound_pos:
return
target_rot = Quaternion2EulerXYZ(target[3:])
current_rot = Quaternion2EulerXYZ(current[3:])
# test_rot = rotx33(current_rot[0]) * roty33(current_rot[1]) * rotz33(current_rot[2])
# print test_rot - cur_pose[0:3, 0:3]
print target_rot[0] - current_rot[0], target_rot[1] - current_rot[1]
# print current_rot
for i in range(2):
# print np.abs(np.min([target_rot[i] - current_rot[i], target_rot[i] - current_rot[i] - 2*np.pi,
# target_rot[i] - current_rot[i] + 2*np.pi]))
if np.min([
np.abs(target_rot[i] - current_rot[i]),
np.abs(target_rot[i] - current_rot[i] - 2 * np.pi),
np.abs(target_rot[i] - current_rot[i] + 2 * np.pi)
]) > self.reaching_bound_rot:
return
# passed all test
# record time taken
self.time_taken[int(
self.current_target)] = time.time() - self.current_time
self.current_time = time.time()
self.current_target = self.current_target + 1
if self.current_target == self.num_target:
self.end_game = True
return
self.set_target(self.current_target)
def log_data(self, IK_method, subject):
with open("../TimeTaken.csv", 'a') as writeFile:
writer = csv.writer(writeFile)
data = [IK_method]
for i in range(self.num_target):
data.append(str(self.time_taken[i]))
data.append(subject)
data.append(str(datetime.datetime.now()))
writer.writerow(data)
