def __init__(self):
# Loads the robot model, which contains the robot's kinematics information
self.robot = URDF.from_parameter_server()
# This is where we hold general info about the joints
self.num_joints = 0
self.link = []
self.joint = []
self.joint_names = []
self.joint_axes = []
self.joint_index = []
self.cartesian = False
self.ik_command = False
# Prepare general information about the robot
self.get_joint_info()
# This is where we'll hold the most recent joint angle information we receive on the topic
self.current_joint_state = JointState()
self.secondary_objective = False
self.q0_target = 0
self.x_desired = tf.transformations.identity_matrix()
self.cartesian_command = CartesianCommand()
self.q_position = []
# Subscribes to information about what the current joint values are.
rospy.Subscriber("/joint_states", JointState, self.joint_callback)
rospy.Subscriber("/cartesian_command", CartesianCommand,
self.cartesian_callback)
rospy.Subscriber("ik_command", geometry_msgs.msg.Transform,
self.ik_callback)
def go_to_pose(self, name, T, secondary_objective, q0_target, timeout,
points):
msg = l1l11lll1_opy_(T)
l1l1l11ll_opy_ = CartesianCommand()
l1l1l11ll_opy_.x_target = msg
l1l1l11ll_opy_.secondary_objective = secondary_objective
l1l1l11ll_opy_.q0_target = q0_target
start_time = time.time()
l1l11ll1l_opy_ = False
index = 0
while not l1l11ll1l_opy_ and not rospy.is_shutdown():
index += 1
self.l1l1ll1l1_opy_.publish(l1l1l11ll_opy_)
try:
(l1l1lllll_opy_,
rot) = self.listener.lookupTransform(self.robot.get_root(),
self.l111l11ll_opy_,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print l11ll_opy_(u"ࠢࡕࡈࠣࡉࡽࡩࡥࡱࡶࡳࡳࠧࠢࣕ")
continue
l1l1l1l1l_opy_ = numpy.dot(
tf.transformations.translation_matrix(l1l1lllll_opy_),
tf.transformations.quaternion_matrix(rot))
if (is_same(T, l1l1l1l1l_opy_)):
if secondary_objective:
if self.robot.name == l11ll_opy_(u"ࠨࡷࡵ࠹ࠬࣖ") and index > 30:
return points
l1l11ll1l_opy_ = True
else:
if abs(self.l1l1l1ll1_opy_ - q0_target) < 10e-3:
#print name + l11ll_opy_ (u"ࠤ࠽ࠤࡕࡇࡓࡔࡇࡇࠦࣗ")
return points
l1l11ll1l_opy_ = True
else:
#print name + l11ll_opy_ (u"ࠥ࠾ࠥࡖࡁࡔࡕࡈࡈࠧࣘ")
return points
l1l11ll1l_opy_ = True
if (time.time() - start_time > timeout):
#print name + l11ll_opy_ (u"ࠦࠦࡒࡰࡤࡲࡸࠥࡺࡰࠣࡸࡴࡵࠠࡱࡱࠥࡺࠡࡴࡨࡥࡨࠠࡥࡧࡶࡷࡤࠡࡲࡲࡷࡪࠨࣙ")
#print l11ll_opy_ (u"ࠧࡘࡣࡱࡷࠤࡹࡵࠢࡷࡳࡴࠦࡰࡰࡪࠤࡹࡵࠠࡳࡧࡤࡧࠦࡤࡦࡵࡶࡪࡪࠠࡱࡱࡶࡩ࠳ࠦࡇࡳࡣࡧࡩࡷࠦࡴࡪࡨࡨࠥࡵࡵࡵࠤࣚ")
return 0
l1l11ll1l_opy_ = True
else:
rospy.sleep(0.1)
return 0
def timer_callback(self, event):
if not self.cc_mode: return
self.mutex.acquire()
if self.ee_tracking:
msg = CartesianCommand()
msg.x_target = convert_to_trans_message(self.x_target)
msg.secondary_objective = False
self.pub_command.publish(msg)
if self.red_tracking:
msg = CartesianCommand()
msg.x_target = convert_to_trans_message(self.x_current)
msg.secondary_objective = True
msg.q0_target = self.q0_desired
self.pub_command.publish(msg)
self.mutex.release()
def go_to_pose(self, name, T, secondary_objective, q0_target, timeout, points):
msg = l1lll1111_opy_(T)
l1lll1ll1_opy_ = CartesianCommand()
l1lll1ll1_opy_.x_target = msg
l1lll1ll1_opy_.secondary_objective = secondary_objective
l1lll1ll1_opy_.q0_target = q0_target
start_time = time.time()
done = False
index = 0
while not done and not rospy.is_shutdown():
index += 1
self.l1lll1lll_opy_.publish(l1lll1ll1_opy_)
try:
(trans,rot) = self.listener.lookupTransform(self.robot.get_root(),self.l1lll1l1l_opy_,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print(l1l1ll11_opy_ (u"ࠣࡖࡉࠤࡊࡾࡣࡦࡲࡷࡴࡴࠡࠣࢉ"))
continue
l1lll111l_opy_ = numpy.dot(tf.transformations.translation_matrix(trans),
tf.transformations.quaternion_matrix(rot))
if (is_same(T, l1lll111l_opy_)):
if secondary_objective:
if self.robot.name == l1l1ll11_opy_ (u"ࠩࡸࡶ࠺࠭ࢊ") and index>30:
return points
else:
if abs(self.l1ll1l1ll_opy_-q0_target)< 10e-3:
#print(name + l1l1ll11_opy_ (u"ࠥ࠾ࠥࡖࡁࡔࡕࡈࡈࠧࢋ"))
return points
else:
#print(name + l1l1ll11_opy_ (u"ࠦࠦࡐࡂࡕࡖࡉࡉࠨࢌ"))
return points
if (time.time() - start_time > timeout) :
#print(name + l1l1ll11_opy_ (u"ࠧࡀࠠࡓࡱࡥࡳࡹࠦࡴࡰࡱࠤࡹࡵࠡࡲࡲࠦࡴࡰࠢࡵࡩࡦࡩࡨࠡࡦࡨࡷࡸࡥࡥࠢࡳࡳࡸࠢࢍ"))
#print(l1l1ll11_opy_ (u"ࠨࡒࡰࡤࡲࡸࠥࡺࡰࠣࡸࡴࡵࠠࡱࡱࠥࡺࠡࡴࡨࡥࡨࠠࡥࡧࡶࡷࡤࠡࡲࡲࡷࡪ࠴ࠠࡈࡴࡤࡨࡪࡸࠠࡵࡰࡩࡩࠦࡶࡶࠥࢎ"))
return 0
else:
rospy.sleep(0.1)
return 0
def __init__(self):
self.mutex = Lock()
self.start = False
foo = CartesianCommand()
#Loads the robot model, which contains the robot's kinematics information
self.robot = URDF.from_parameter_server()
print('start')
#Subscribes to information about what the current joint values are.
rospy.Subscriber("joint_states", JointState, self.callback)
# Publishes Cartesian goals
self.pub_command = rospy.Publisher("/cartesian_command",
CartesianCommand,
queue_size=1)
# Publishes IK command
self.ik_command = rospy.Publisher("/ik_command",
geometry_msgs.msg.Transform,
queue_size=1)
# Publisher to set robot position
self.pub_reset = rospy.Publisher("/joint_command",
JointState,
queue_size=1)
#This is where we hold the most recent joint transforms
self.joint_transforms = []
self.x_current = tf.transformations.identity_matrix()
self.R_base = tf.transformations.identity_matrix()
#Create "Interactive Marker" that we can manipulate in RViz
self.cc_mode = True
print('self.robot', self.robot)
self.init_marker()
self.ee_tracking = 0
self.red_tracking = 0
self.q_current = []
self.x_target = tf.transformations.identity_matrix()
self.q0_desired = 0
self.q0_base = 0
self.angle_base = 0
self.delta = 0
self.timer = rospy.Timer(rospy.Duration(0.01), self.timer_callback)
print('self.ee_tracking', self.ee_tracking)
self.start = True
def get_ik_command(self, command):
self.mutex.acquire()
#start timer
# print("start")
command_cc = CartesianCommand()
command_cc.x_target.translation = command.translation
command_cc.x_target.rotation = command.rotation
b_T_ee_des_T = tf.transformations.translation_matrix(
(command.translation.x, command.translation.y,
command.translation.z))
# print("desired T in base coordinate",b_T_ee_des_T)
b_T_ee_des_R = tf.transformations.quaternion_matrix([
command.rotation.x, command.rotation.y, command.rotation.z,
command.rotation.w
])
# print("desired R in base coordinate ",b_T_ee_des_R)
b_T_ee_des = numpy.dot(b_T_ee_des_T, b_T_ee_des_R)
p = 1
for i in range(3):
#initial random q
time_out = 10 #10 sec
time_start = time.time()
self.q_current = random.sample(numpy.arange(0.0, 2 * 3.14, 0.1),
self.num_joints)
joint_transforms, b_T_ee_curr = self.forward_kinematics(
self.q_current)
error = 1
#(time.time() - time_start < time_out) and
while (time.time() - time_start < time_out) and (error > 0.01):
# print("while loop start")
joint_transforms, b_T_ee_curr = self.forward_kinematics(
self.q_current)
# print("start cartisain command")
ee_curr_T_b = numpy.linalg.inv(b_T_ee_curr)
curr_T_des = ee_curr_T_b.dot(b_T_ee_des)
# print("desired T in ee coordinate ",curr_T_des)
translation_ee = tf.transformations.translation_from_matrix(
curr_T_des)
rotation_ee = self.rotation_from_matrix(curr_T_des)
rotation_ee = rotation_ee[0] * rotation_ee[1]
Vee = numpy.array([translation_ee, rotation_ee]).reshape(6)
J = self.get_jacobian(b_T_ee_curr, joint_transforms)
J_plus = numpy.linalg.pinv(J)
q_desired = J_plus.dot(Vee)
velocity = JointState()
velocity.name = self.joint_names
velocity.velocity = q_desired
# print("caculate q_curent intergration")
self.q_current += p * q_desired
error = numpy.max(numpy.abs(b_T_ee_curr - b_T_ee_des))
print(i, error, (time.time() - time_start))
if error < 0.01:
q = JointState()
q.name = self.joint_names
q.position = self.q_current
self.joint_command_pub.publish(q)
# print("published")
break
# print("quit")
pass
self.mutex.release()