def __init__(self):
rospy.loginfo("Creating VelocityController class")
# Create KDL model
with cl.suppress_stdout_stderr(): # Eliminates urdf tag warnings
self.robot = URDF.from_parameter_server()
self.kin = KDLKinematics(self.robot, "base", "right_hand")
self.names = self.kin.get_joint_names()
# Limb interface
self.arm = intera_interface.Limb("right")
self.hand = intera_interface.gripper.Gripper('right')
# Grab M0 and Blist from saywer_MR_description.py
self.M0 = s.M #Zero config of right_hand
self.Blist = s.Blist #6x7 screw axes mx of right arm
# Shared variables
self.mutex = threading.Lock()
self.time_limit = rospy.get_param("~time_limit", TIME_LIMIT)
self.damping = rospy.get_param("~damping", DAMPING)
self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT)
self.q = np.zeros(7) # Joint angles
self.qdot = np.zeros(7) # Joint velocities
self.T_goal = np.array(self.kin.forward(self.q)) # Ref se3
# Subscriber
self.ref_pose_sub = rospy.Subscriber('ref_pose', Pose, self.ref_pose_cb)
self.hand.calibrate()
self.r = rospy.Rate(100)
while not rospy.is_shutdown():
self.calc_joint_vel()
self.r.sleep()
def __init__(self):
rospy.loginfo("Creating IK Controller class")
# setup flags and useful vars
self.running_flag = False
self.step_scale = rospy.get_param("~scale", STEP_SCALE)
self.freq = rospy.get_param("~freq", FREQ)
self.arm = rospy.get_param("~arm", ARM)
self.q = np.zeros(7)
with cl.suppress_stdout_stderr():
self.urdf = URDF.from_parameter_server()
self.kin = KDLKinematics(self.urdf, "base", "%s_hand"%self.arm)
self.goal = np.array(self.kin.forward(self.q))
self.mutex = threading.Lock()
self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT)
self.q_sim = np.zeros(7)
self.damping = rospy.get_param("~damping", DAMPING)
self.joint_names = self.kin.get_joint_names()
self.qdot_dict = {}
self.limb_interface = intera_interface.Limb()
self.ep = EndpointState()
# create all subscribers, publishers, and timers
self.int_timer = rospy.Timer(rospy.Duration(1/float(self.freq)), self.ik_step_timercb)
self.actual_js_sub = rospy.Subscriber("robot/joint_states", JointState, self.actual_js_cb)
self.endpoint_update_cb = rospy.Subscriber("robot/limb/right/endpoint_state", EndpointState, self.endpoint_upd_cb)
def __init__(self):
rospy.loginfo("Creating IK Controller class")
# setup flags and useful vars:
self.running_flag = False
self.freq = rospy.get_param("~freq", FREQ)
self.arm = rospy.get_param("~arm", ARM)
self.base = rospy.get_param("~base", BASE)
self.target = rospy.get_param("~target", TARGET)
self.step_scale = rospy.get_param("~scale", STEP_SCALE)
self.tolerance = rospy.get_param("~tolerance", TOLERANCE)
self.damping = rospy.get_param("~damping", DAMPING)
self.center_x = rospy.get_param("~center_x", CENTER_X)
self.center_y = rospy.get_param("~center_y", CENTER_Y)
self.center_z = rospy.get_param("~center_z", CENTER_Z)
self.range_x = rospy.get_param("~range_x", RANGE_X)
self.range_y = rospy.get_param("~range_y", RANGE_Y)
self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT)
with cl.suppress_stdout_stderr():
self.urdf = URDF.from_parameter_server()
self.kin = KDLKinematics(self.urdf, "base", "%s_hand"%self.arm)
self.q = np.zeros(7)
self.q_sim = np.zeros(7)
self.limb_interface = intera_interface.Limb()
self.center_pos()
self.goal = np.array(self.kin.forward(self.q))
self.js = JointState()
self.js.name = self.kin.get_joint_names()
self.pose = PoseStamped()
self.mutex = threading.Lock()
self.joint_cmd = JointCommand()
self.joint_cmd.names = self.kin.get_joint_names()
self.joint_cmd.mode = 2
# create all services:
self.toggle_server = rospy.Service("toggle_controller", SetBool, self.toggle_handler)
self.reset_server = rospy.Service("reset", Empty, self.reset_handler)
self.random_server = rospy.Service("random", Empty, self.random_handler)
# create all subscribers, timers, and publishers:
self.ref_sub = rospy.Subscriber("ref_pose", PoseStamped, self.refcb)
self.actual_js_sub = rospy.Subscriber("robot/joint_states", JointState, self.actual_js_cb)
self.js_pub = rospy.Publisher("joint_states", JointState, queue_size=3)
self.pose_pub = rospy.Publisher("pose", PoseStamped, queue_size=3)
self.joint_cmd_timeout_pub = rospy.Publisher("robot/limb/right/joint_command_timeout", Float64, queue_size=3)
self.center_pos()
self.joint_cmd_pub = rospy.Publisher("robot/limb/right/joint_command", JointCommand, queue_size=3)
self.int_timer = rospy.Timer(rospy.Duration(1/float(self.freq)), self.ik_step_timercb)
return
def get_and_set_params(self):
self.freq = rospy.get_param("freq", FREQ)
self.dt = 1 / float(self.freq)
if rospy.has_param("robot_description"):
with cl.suppress_stdout_stderr():
self.robot = URDF.from_parameter_server()
else:
rospy.logwarn("Need to add robot model to parameter server")
rospy.sleep(2.)
rospy.signal_shutdown("No robot model")
with warnings.catch_warnings():
warnings.simplefilter("ignore")
self.kin = KDLKinematics(self.robot, FRAME, EE_FRAME)
# build empty JointState message:
self.js = JointState()
self.js.name = self.kin.get_joint_names()
# build PoseStamped message:
self.p = PoseStamped()
self.p.header.frame_id = FRAME
# generate initial target:
self.generate_new_target()
return
def __init__(self):
rospy.loginfo("Creating VelocityController class")
# Create KDL model
with cl.suppress_stdout_stderr(): # Eliminates urdf tag warnings
self.robot = URDF.from_parameter_server() # sawyer's URDF
# self.kin = KDLKinematics(self.robot, "base", "right_gripper") # kinematics to custom joint frame
self.kin = KDLKinematics(self.robot, "base", "right_gripper_tip")
self.names = self.kin.get_joint_names()
if rospy.has_param('vel_calc'):
rospy.delete_param('vel_calc')
self.limb = intera_interface.Limb("right")
#self._limb = Limb()
self.gripper = intera_interface.gripper.Gripper('right')
# Grab M0 and Blist from saywer_MR_description.py
self.M0 = s.M #Zero config of right_hand
self.Blist = s.Blist #6x7 screw axes mx of right arm
self.Kp = 50.0 * np.eye(6)
self.Ki = 0.0 * np.eye(6)
self.Kd = 100.0 * np.eye(6) # add D-gain
self.it_count = 0
self.int_err = 0
self.der_err = 0
self.int_anti_windup = 10
self.rate = 100.0 #Hz
self.sample_time = 1.0 / self.rate * 2.5 #ms
self.USE_FIXED_RATE = False # If true -> dT = 0.01, Else -> function call time difference
self.current_time = time.time()
self.last_time = self.current_time
self.last_error = [0.0, 0.0, 0.0, 0.0, 0.0,
0.0] # error in cartesian space
# Shared variables
self.mutex = threading.Lock()
self.time_limit = rospy.get_param("~time_limit", TIME_LIMIT)
self.damping = rospy.get_param("~damping", DAMPING)
self.joint_vel_limit = rospy.get_param("~joint_vel_limit",
JOINT_VEL_LIMIT)
self.q = np.zeros(7) # Joint angles
self.qdot = np.zeros(7) # Joint velocities
self.T_goal = np.array(self.kin.forward(self.q)) # Ref se3
self.cur_frame = pm.fromMatrix(self.T_goal) # -> PyKDL frame
# robot @ its original pose : position (0.3161, 0.1597, 1.151) , orientation (0.337, 0.621, 0.338, 0.621)
self.original_goal = self.T_goal.copy() # robot @ its home pose :
print('Verifying initial pose...')
print(self.T_goal)
self.isCalcOK = False
self.isPathPlanned = False
self.traj_err_bound = float(1e-2) # in meter
self.plan_time = 2.5 # in sec. can this be random variable?
self.rand_plan_min = 5.0
self.rand_plan_max = 9.0
# Subscriber
self.ee_position = [0.0, 0.0, 0.0]
self.ee_orientation = [0.0, 0.0, 0.0, 0.0]
self.ee_lin_twist = [0.0, 0.0, 0.0]
self.ee_ang_twist = [0.0, 0.0, 0.0]
rospy.Subscriber('/demo/target/', Pose, self.ref_poseCB)
self.gripper.calibrate()
# path planning
self.num_wp = int(0)
self.cur_wp_idx = int(0) # [0:num_wp - 1]
self.traj_list = [None for _ in range(self.num_wp)]
self.traj_elapse = 0.0 # in ms
self.r = rospy.Rate(self.rate)
# robot chain for the ik solver of PyKDL
# This constructor parses the URDF loaded in rosparm urdf_param into the
# needed KDL structures.
self._base = self.robot.get_root()
self._tip = "right_gripper_tip"
self.sawyer_tree = kdl_tree_from_urdf_model(
self.robot) # sawyer's kinematic tree
self.sawyer_chain = self.sawyer_tree.getChain(self._base, self._tip)
# KDL solvers
self.ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self.sawyer_chain)
self._num_jnts = 7
self._joint_names = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
rospy.loginfo('Check the URDF of sawyer')
self.print_robot_description()
rospy.loginfo('Check the sanity of kinematic chain')
self.print_kdl_chain()
self.prev_frame = PyKDL.Frame() # initialize as identity frame
self.init_frame = PyKDL.Frame(
) # frame of the start pose of the trajectory
self._frame1 = PyKDL.Frame()
self._frame2 = PyKDL.Frame()
self.pose1 = Pose()
self.pose2 = Pose()
rospy.Subscriber("test", itzy, callback=self.msgggg)
self.vr0_T_saw0 = np.identity(4)
self.btn_state = 0
self.btn_state_2 = 0
self._gripper = intera_interface.Gripper()
rospy.Subscriber('vive_right', Joy, callback=self.grip_open_close)
# control loop
rospy.Subscriber('/robot/limb/right/endpoint_state', EndpointState,
self.endpoint_poseCB)
while not rospy.is_shutdown():
# if rospy.has_param('vel_calc'):
# if not self.isPathPlanned: # if path is not planned
# self.path_planning() # get list of planned waypoints
# # self.calc_joint_vel_2()
self.calc_joint_vel_3()
self.r.sleep()
def right_char():
# Create KDL model
with cl.suppress_stdout_stderr():
robot = URDF.from_parameter_server()
kin = KDLKinematics(robot, "base", "right_hand")
# Screw axis for joint 1: /right_upper_shoulder
w1 = np.array([0,0,1]) #z-axis of SO(3)
q1 = np.array([0.06402724, -0.25902738, 0]) #frame origin
v1 = -np.cross(w1,q1)
S1 = np.append(w1,v1)
# Screw axis for joint 2: /right_lower_shoulder
w2 = np.array([np.sqrt(2)/2, np.sqrt(2)/2, 0])
q2 = np.array([0.11281752, -0.30781784, 0.399976])
v2 = -np.cross(w2,q2)
S2 = np.append(w2,v2)
# Screw axis for joint 3: /right_upper_elbow
w3 = np.array([np.sqrt(2)/2, -np.sqrt(2)/2, 0])
q3 = np.array([0.18494228, -0.37994287, 0.399976])
v3 = -np.cross(w3,q3)
S3 = np.append(w3,v3)
# Screw axis for joint 4: /right_lower_elbow
w4 = np.array([np.sqrt(2)/2, np.sqrt(2)/2, 0])
q4 = np.array([0.3705009, -0.56550217, 0.330976])
v4 = -np.cross(w4,q4)
S4 = np.append(w4,v4)
# Screw axis for joint 5: /right_upper_forearm
w5 = np.array([np.sqrt(2)/2, -np.sqrt(2)/2, 0])
q5 = np.array([0.44374996, -0.63875149, 0.330976])
v5 = -np.cross(w5,q5)
S5 = np.append(w5,v5)
# Screw axis for joint 6: /right_lower_forearm
w6 = np.array([np.sqrt(2)/2, np.sqrt(2)/2, 0])
q6 = np.array([0.63516341, -0.83016565, 0.320976])
v6 = -np.cross(w6,q6)
S6 = np.append(w6,v6)
# Screw axis for joint 7: /right_wrist
w7 = np.array([np.sqrt(2)/2, -np.sqrt(2)/2, 0])
q7 = np.array([0.71716997, -0.91217251, 0.320976])
v7 = -np.cross(w7,q7)
S7 = np.append(w7,v7)
# Transform from /base to /right_hand
M0 = np.zeros((4,4)) #Tsb - items in body frame wrt space frame
M0[0:3,0:3] = np.array([[ 0, np.sqrt(2)/2, np.sqrt(2)/2],
[ 0, np.sqrt(2)/2, -np.sqrt(2)/2],
[-1, 0, 0]])
M0[0:3,3] = np.array([0.7974618, -0.99246463, 0.320976]) #ee pos @ zero
M0[3,3] = 1
Slist = np.array([S1,S2,S3,S4,S5,S6,S7]).T #6x7 matrix
parts = [M0, Slist]
return parts
