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
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 initialize(self):
cmd_msg = ObjectCmd()
self.cmd_msg = cmd_msg
self.t_start = rospy.get_time()
self.t_old = 0
self.q_old = np.zeros((6, 1))
self.pos_old = np.zeros((3, 1))
self.J_old = np.zeros((3, 6))
self.model, self.tip_body = model_generator()
self.radius = 0.2
self.omega = math.pi / 36
self.lin_vel = 0.1
self.ylim = [-0.4, 0.4]
self.zlim = [0.1, 0.5]
self.dir = '+'
self.velY = self.lin_vel
self.velZ = self.lin_vel
self.force = -25
self.errcount = 0
self.initial_pos = np.array(
fwd_kinematics(
self.model,
np.array([0, math.pi / 6, 0, -math.pi / 2, 0,
math.pi / 3]).reshape(6, 1),
self.tip_body)).reshape(3, 1)
self.center = self.initial_pos - np.array([0, self.radius, 0]).reshape(
3, 1)
self.idx = 0
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 __init__(self, a_name):
"""
Constructor
:param a_name:
"""
super(Object, self).__init__(time_out=0.1) # Set duration of Watchdog expiry
self._name = ''
self._state = ObjectState()
self._cmd = ObjectCmd()
self._pub = None
self._sub = None
self.pub_flag = True
self._active = False
self._pose_cmd_set = False # Flag to check if a Pose command has been set from the Object
self._wrench_cmd_set = False # Flag to check if a Wrench command has been set from the Object
rate = rospy.Rate(1000)
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
# \author <http://www.aimlab.wpi.edu>
# \author <*****@*****.**>
# \author Adnan Munawar
# \version 0.1
# */
# //==============================================================================
from ambf_msgs.msg import ObjectState, ObjectCmd
import rospy
from geometry_msgs.msg import Pose
from PyKDL import Rotation, Frame, Vector
global openhmd_state, occulus_state, occulus_state_valid, openhmd_state_valid
openhmd_state = Pose()
occulus_state = ObjectState()
occulus_cmd = ObjectCmd()
occulus_state_valid = False
openhmd_state_valid = False
def openhmd_cb(msg):
global openhmd_state, openhmd_state_valid
openhmd_state = msg
openhmd_state_valid = True
def occulus_cb(msg):
global occulus_state, occulus_state_valid
occulus_state = msg
occulus_state_valid = True
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()
rate = rospy.Rate(1000)
K_lin = 100.0
D_lin = 20.0
K_ang = 5
D_ang = 1
last_delta_pos = Vector(0, 0, 0)
delta_pos = Vector(0, 0, 0)
last_pos = Vector(0, 0, 0)
m_drot_prev = Rotation.RPY(0, 0, 0)
m_drot = Rotation.RPY(0, 0, 0)
last_rot = Rotation.RPY(0, 0, 0)
cmd_msg = ObjectCmd()
cmd_msg.enable_position_controller = False
dt = 0.001
cur_time = rospy.Time.now().to_sec()
torque = Vector(0, 0, 0)
last_torque = Vector(0, 0, 0)
d_torque = Vector(0, 0, 0)
# Main Loop
while not rospy.is_shutdown():
App.update()
last_time = cur_time
cur_time = rospy.Time.now().to_sec()
dt = cur_time - last_time
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()