def initialize_controller(self): cmd_msg = ObjectCmd() cmd_msg.enable_position_controller = False cmd_msg.position_controller_mask = [False] self.NJoints = get_joint_num() self.cmd_msg = cmd_msg self.t0 = rospy.get_time() self.prev_time = 0 ## Joint Space self.prev_state = np.zeros(self.NJoints) self.prev_vel = np.zeros(self.NJoints) self.prev_vel1 = np.zeros(self.NJoints) self.prev_vel2 = np.zeros(self.NJoints) self.prev_vel3 = np.zeros(self.NJoints) self.prev_vel4 = np.zeros(self.NJoints) self.prev_wt_vel = np.zeros(self.NJoints) self.prev_J = np.zeros((3,self.NJoints)) ## Task Space self.prev_x = np.zeros(3) self.prev_velX = np.zeros(3) # Set end-effector desired velocity here self.X_SPEED = np.zeros(3) self.X_SPEED[0] = 1 self.x_speed_local = self.X_SPEED
def main():
sub = rospy.Subscriber('/ambf/env/base/State', ObjectState, get_joint_values, queue_size=1)
pub = rospy.Publisher('/ambf/env/base/Command', ObjectCmd, queue_size=1)
rospy.init_node('gravity_compensation_controller')
rate = rospy.Rate(1000) #1000hz
cmd_msg = ObjectCmd()
cmd_msg.enable_position_controller = False
cmd_msg.position_controller_mask = [False]
while not rospy.is_shutdown():
# #current pose
# q = pos # print('current pos:', q[1]*3.14/180)
# q_dot = np.zeros(7)
# q_ddot = np.zeros(7)
# G = get_G(q)
G = [-4.61852778e-17, 8.37889941e+00, -3.12902711e+00, 1.13688986e+01, 7.97401873e-03, 3.72971015e-01, -4.57673510e-02] # G = Inverse_dynamics_calc_func(q, q_dot, q_ddot)
print('tau', G)
#define header
Header = std_msgs.msg.Header()
Header.stamp = rospy.Time.now()
cmd_msg.header = Header
# cmd_msg.joint_cmds = [ tau[0], tau[1],tau[2],tau[3],tau[4],tau[5],tau[6]]#,11.65* 0, 0, 0, 0, 0]
cmd_msg.joint_cmds = G
# # print(" torque is ", cmd_msg.joint_cmds)
pub.publish(cmd_msg)
rate.sleep()
def initialize_controller(self): cmd_msg = ObjectCmd() cmd_msg.enable_position_controller = False cmd_msg.position_controller_mask = [False] self.NJoints = get_joint_num() self.cmd_msg = cmd_msg # what is the reason for this? self.t0 = rospy.get_time() self.prev_time = 0 ## Joint Space self.prev_state = np.zeros(self.NJoints) self.prev_vel = np.zeros(self.NJoints) # creating 5 velocity instances for taking the avg of them in the weighted manner self.prev_vel1 = np.zeros(self.NJoints) self.prev_vel2 = np.zeros(self.NJoints) self.prev_vel3 = np.zeros(self.NJoints) self.prev_vel4 = np.zeros(self.NJoints) self.prev_wt_vel = np.zeros(self.NJoints) ## Task Space self.prev_x = np.zeros(3) # Cartesian velocity of the end-effector in the task space (x,y,z) self.prev_velX = np.zeros(3) # Set end-effector desired velocity here self.X_SPEED = np.zeros(3) self.X_SPEED[0] = 0.1 # desired velocity of the end-effector in the x-direction is set to 1 m/s self.x_speed_local = self.X_SPEED
K_lin = 150.0
D_lin = 20.0
ki = 2
last_delta_pos = 0
delta_pos = 0
last_pos = 0
last_vel = 0
delta_delta_pos = 0
velocity_val = 0
change_vel = 0
cur_pos = 0
T = 0
cmd_msg = ObjectCmd()
cmd_msg.enable_position_controller = False
dt = 0.001
cur_time = rospy.Time.now().to_sec()
# Main Loop
while not rospy.is_shutdown():
App.update()
# Determining time
last_time = cur_time
cur_time = rospy.Time.now().to_sec()
dt = cur_time - last_time
if dt < 0.001:
dt = 0.001
if active:
def main():
sub = rospy.Subscriber('/ambf/env/base/State',
ObjectState,
get_joint_values,
queue_size=1)
pub = rospy.Publisher('/ambf/env/base/Command', ObjectCmd, queue_size=1)
rospy.init_node('kuka_gravity_compensation')
# sub = rospy.Subscriber('/ambf/env/mtm/TopPanel/State', ObjectState, get_joint_values, queue_size=1)
# pub = rospy.Publisher ('/ambf/env/mtm/TopPanel/Command', ObjectCmd, queue_size=1)
# rospy.init_node('MTM_gravity_compensation')
rate = rospy.Rate(1000) #1000hz
# Kp and Kd values
Kp = 0 #1
Kd = 0 #00.001
Ki = 0.000
# calculating the time difference
prev_time = rospy.Time.now().to_sec(
) - 0.06 #adding loop time for initial dt
cmd_msg = ObjectCmd()
cmd_msg.enable_position_controller = False
cmd_msg.position_controller_mask = [False]
qGoal = 0 * 3.1457 / 180
qdotGoal = 0
prev_pos = pos
errSum = 0
while not rospy.is_shutdown():
curr_time = rospy.Time.now().to_sec()
dt = curr_time - prev_time
prev_time = curr_time
curr_pos = pos
print 'current pos:', curr_pos
# print 'current pos:', curr_pos[1]*3.14/180
q_dot = (curr_pos - prev_pos) / dt
# curr_pos = [0.1]*7
# curr_pos[1]=0.5
G = get_G(curr_pos)
err = curr_pos[1] - qGoal
errSum += err
tau = -(Kp * (err) + Kd *
(q_dot[1] - qdotGoal) + Ki * errSum) + G # +qgoal #G[1] #5.85*
# print 'tau', tau
#define header
Header = std_msgs.msg.Header()
Header.stamp = rospy.Time.now()
cmd_msg.header = Header
cmd_msg.joint_cmds = [
tau[0], tau[1], tau[2], tau[3], tau[4], tau[5], tau[6]
] #
# cmd_msg.joint_cmds = [tau[0],tau[1],tau[5],tau[2],tau[3],tau[6],tau[7],tau[8],tau[9]]# for theclosed loop MTM
# cmd_msg.joint_cmds = [tau[0],tau[1],tau[5],tau[2],tau[3],tau[6],tau[7],tau[8],tau[9]]# for theclosed loop MTM
# cmd_msg.joint_cmds = [ tau[0],tau[1],tau[2],tau[3],tau[4],tau[5],tau[6]]# for the openloop system
# cmd_msg.joint_cmds = [tau[0],tau[1],tau[2],0,0,tau[3],tau[4],tau[5],tau[6]]# for closed loop using open loop
# # print(" torque is ", cmd_msg.joint_cmds)
pub.publish(cmd_msg)
rate.sleep()
def controller():
global active, Kp, Kd
sub = rospy.Subscriber('/ambf/env/base/State',
ObjectState,
get_joint_values,
queue_size=1)
pub = rospy.Publisher('/ambf/env/base/Command', ObjectCmd, queue_size=1)
rospy.init_node('ambf_gravitycontrol_test')
rate = rospy.Rate(1000) #1000hz
# calculating the time difference
dt = 0.001
cur_time = rospy.Time.now().to_sec()
cmd_msg = ObjectCmd()
cmd_msg.enable_position_controller = False
cmd_msg.position_controller_mask = [False]
while not rospy.is_shutdown():
#Read time from ROS
last_time = cur_time
cur_time = rospy.Time.now().to_sec()
dt = cur_time - last_time
print "loop freq is ", 1 / dt
if dt < 0.001:
dt = 0.001
if active:
# Reading current position value of all the links
t1 = rospy.Time.now().to_sec()
prev_pos = np.asarray(Q_vals)
t2 = rospy.Time.now().to_sec()
cur_pos = np.asarray(Q_vals)
time_btwn_poses = t2 - t1
pose_diff = cur_pos - prev_pos
velocity_pid = pose_diff * dt
velocity_val = np.zeros(7)
accel_val = np.zeros(7)
torque_gravity_comp = Inverse_dynamics_calc_func(
np.zeros(7), velocity_val, accel_val)
torque_pid = Kd * (velocity_pid - velocity_val) + Kp * pose_diff
torque = 10 * torque_gravity_comp + torque_pid
# print(" torque is ", len(torque))
#define header
Header = std_msgs.msg.Header()
Header.stamp = rospy.Time.now()
cmd_msg.header = Header
# cmd_msg.joint_cmds = [torque[0], torque[1], torque[2], torque[3], torque[4], torque[5], torque[6]]
cmd_msg.joint_cmds = [0, torque[1], 0, 0, 0, 0, 0]
print(" torque is ", cmd_msg.joint_cmds)
pub.publish(cmd_msg)
rate.sleep()
rospy.spin()