def test_relative(self):
"""Test robot's ability to position its gripper relative to a given marker."""
goal = Pose()
roll = -math.pi / 2
pitch = 0
yaw = -math.pi / 2
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
goal.orientation.x = quaternion[0]
goal.orientation.y = quaternion[1]
goal.orientation.z = quaternion[2]
goal.orientation.w = quaternion[3]
while True:
end = user_input("Zelite li nastaviti? d/n")
if end != 'd':
break
trans = self.transformations()
goal.position.x = trans[0]
goal.position.y = trans[1]
goal.position.z = trans[2]
offset_x = float(user_input("X?"))
offset_y = float(user_input("Y?"))
offset_z = float(user_input("Z?"))
# Uncomment for testing movement speed as well
# brzina = float(user_input("Brzina?"))
# self.left_arm.set_joint_position_speed(brzina)
goal.position.x += offset_x
goal.position.y += offset_y
goal.position.z += offset_z
goal_final = baxterGoal(id=1, pose=goal)
self.left_client.send_goal_and_wait(goal_final)
result = self.left_client.get_result()
评论列表
文章目录
