Skip to content

Commit 9416d44

Browse files
committed
made a point cloud for controller
1 parent fdf3809 commit 9416d44

File tree

1 file changed

+148
-0
lines changed

1 file changed

+148
-0
lines changed

scripts/Controller.py

Lines changed: 148 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,148 @@
1+
import numpy as np
2+
import rospy
3+
4+
from std_msgs.msg import Float64, Empty
5+
from trac_ik_python.trac_ik import IK
6+
from sensor_msgs.msg import JointState
7+
from geometry_msgs.msg import Pose, Point, Quaternion
8+
import tf
9+
10+
class Controller:
11+
def __init__(self):
12+
# initialize publishers, subscribers, tflisteners
13+
self.arm_pub = rospy.Publisher('/goal_dynamixel_position', JointState, queue_size=1)
14+
self.pan_pub = rospy.Publisher('/pan/command', Float64, queue_size=1)
15+
self.tilt_pub = rospy.Publisher('/tilt/command', Float64, queue_size=1)
16+
self.gripper_open_pub = rospy.Publisher('/gripper/open', Empty, queue_size=1)
17+
self.gripper_close_pub = rospy.Publisher('/gripper/close', Empty, queue_size=1)
18+
rospy.Subscriber('/joint_states', JointState, self.get_joint_state)
19+
self.tf_listener = tf.TransformListener()
20+
21+
# global variables
22+
self.current_joint_state = None
23+
self.current_gripper_state = None
24+
25+
# global frames
26+
self.GRIPPER_LINK = "gripper_link"
27+
self.BOTTOM_PLATE_LINK = "bottom_plate"
28+
29+
# other classes
30+
self.ik_solver = IK(self.BOTTOM_PLATE_LINK, self.GRIPPER_LINK)
31+
32+
# predefined variables
33+
self.HOME_POS_MANIPULATOR_01 = [0.004601942375302315, -0.4218447208404541, 1.6260197162628174, -0.1426602154970169, 0.010737866163253784]
34+
self.HOME_POS_MANIPULATOR_02 = [0.0, 0.0, 1.22, -0.142, 0.0]
35+
self.HOME_POS_CAMERA_01 = [0.0, 0.698]
36+
self.HOME_POS_CAMERA_02 = [0.698, 0.0]
37+
self.IK_POSITION_TOLERANCE = 0.01
38+
self.IK_ORIENTATION_TOLERANCE = np.pi/9
39+
self.MIN_CLOSING_GAP = 0.002
40+
41+
def set_camera_angles(self, pan_rad, tilt_rad):
42+
pan_msg = Float64()
43+
pan_msg.data = pan_rad
44+
rospy.loginfo('Going to camera pan: {} rad'.format(pan_rad))
45+
self.pan_pub.publish(pan_msg)
46+
47+
tilt_msg = Float64()
48+
tilt_msg.data = tilt_rad
49+
rospy.loginfo('Going to camera tilt: {} rad'.format(tilt_rad))
50+
self.tilt_pub.publish(tilt_msg)
51+
rospy.sleep(4)
52+
53+
def set_arm_joint_angles(self, joint_target):
54+
joint_state = JointState()
55+
joint_state.position = tuple(joint_target)
56+
self.arm_pub.publish(joint_state)
57+
58+
def get_joint_state(self, data):
59+
self.current_joint_state = data.position[0:5]
60+
self.current_gripper_state = data.positions[7:9]
61+
62+
def open_gripper(self):
63+
empty_msg = Empty()
64+
rospy.loginfo('Opening gripper')
65+
self.gripper_open_pub.publish(empty_msg)
66+
rospy.sleep(4)
67+
68+
def close_gripper(self):
69+
empty_msg = Empty()
70+
rospy.loginfo('Closing gripper')
71+
self.gripper_close_pub.publish(empty_msg)
72+
rospy.sleep(4)
73+
74+
# finds the inverse kinematics solution for the target pose
75+
def compute_ik(self, target_pose):
76+
"""
77+
Parameters
78+
----------
79+
ik_solver: trac_ik_python.trac_ik Ik object
80+
target_pose: type geometry_msgs/Pose
81+
current_joint: list with length the number of joints (i.e. 5)
82+
Returns
83+
----------
84+
IK solution (a list of joint angles for target_pose)
85+
if found, None otherwise
86+
"""
87+
result = self.ik_solver.get_ik(self.current_joint_state,
88+
target_pose.position.x,
89+
target_pose.position.y,
90+
target_pose.position.z,
91+
target_pose.orientation.x,
92+
target_pose.orientation.y,
93+
target_pose.orientation.z,
94+
target_pose.orientation.w,
95+
self.IK_POSITION_TOLERANCE,
96+
self.IK_POSITION_TOLERANCE,
97+
self.IK_POSITION_TOLERANCE,
98+
self.IK_ORIENTATION_TOLERANCE,
99+
self.IK_ORIENTATION_TOLERANCE,
100+
self.IK_ORIENTATION_TOLERANCE)
101+
102+
if result:
103+
rospy.loginfo('IK solution found')
104+
else:
105+
rospy.logerr('No IK solution found')
106+
return result
107+
108+
# Goes to the position given by FRAME and grabs the object from the top
109+
def go_to_pose(self, FRAME):
110+
while not rospy.is_shutdown():
111+
try:
112+
self.tf_listener.waitForTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time.now(), rospy.Duration(5.0))
113+
P, Q = self.tf_listener.lookupTransform(self.BOTTOM_PLATE_LINK, FRAME, rospy.Time(0))
114+
break
115+
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
116+
print(e)
117+
118+
O = tf.transformations.euler_from_quaternion(Q)
119+
Q = tf.transformations.quaternion_from_euler(0, np.pi/2, O[2])
120+
121+
poses = [Pose(Point(P[0], P[1], P[2]+0.20), Quaternion(Q[0], Q[1], Q[2], Q[3])),
122+
Pose(Point(P[0], P[1], P[2]+0.15), Quaternion(Q[0], Q[1], Q[2], Q[3]))]
123+
124+
target_joint = None
125+
self.open_gripper()
126+
for pose in poses:
127+
if self.current_joint_state:
128+
target_joint = self.compute_ik(pose)
129+
else:
130+
print("Joint State Subscriber not working")
131+
return False
132+
133+
if target_joint:
134+
self.set_arm_joint_angles(target_joint)
135+
rospy.sleep(8)
136+
else:
137+
return False
138+
self.close_gripper()
139+
140+
# checks if the object was grasped or not
141+
def check_grasp(self):
142+
if self.current_gripper_state:
143+
if(np.abs(self.current_gripper_state[0]) < self.MIN_CLOSING_GAP \
144+
and np.abs(self.current_gripper_state[1] < self.MIN_CLOSING_GAP)):
145+
return False
146+
else:
147+
print("Joint State Subscriber not working")
148+
return False

0 commit comments

Comments
 (0)