def find_ik_and_execute(self, pose_transformed):
x = pose_transformed.pose.position.x
y = pose_transformed.pose.position.y
z = pose_transformed.pose.position.z
ik_candidate = ik_solver(x, y, z, -90)
# print "========== Find ",len(ik_candidate)," Plan =========="
if not np.isnan(ik_candidate.all()):
for theta_1, theta_2, theta_3, theta_4 in ik_candidate:
# while not rospy.is_shutdown():
try:
if self.execute_fk(theta_1, theta_2, theta_3, theta_4):
# rospy.loginfo("========== Execute Plan ==========")
# print [theta_1, theta_2, theta_3, theta_4]
break
except Exception as e:
# rospy.loginfo(e)
# print "------------- Failed -------------"
# print [theta_1, theta_2, theta_3, theta_4],"\n"
continue
else:
rospy.loginfo("========== Cannot Find Solution ==========")
self._result.state = False
self._as.set_aborted(self._result)
def pick_cb(self, req):
br = tf.TransformBroadcaster()
tf_name = req.tf
print(tf_name)
pose_goal = geometry_msgs.msg.Pose()
pose_goal.position.x = tf_name.position.x
pose_goal.position.y = tf_name.position.y
pose_goal.position.z = tf_name.position.z - 0.005
print "Your object's position : ", pose_goal.position
degree = -90
for j in range(20):
joint_value = ik_4dof.ik_solver(pose_goal.position.x,
pose_goal.position.y,
pose_goal.position.z, degree)
if len(joint_value) > 0:
for joint in joint_value:
joint = list(joint)
# determine gripper state
joint.append(0)
try:
self.move_group_arm.go(joint, wait=True)
self.move_group_arm.stop()
except:
rospy.loginfo(
str(joint) + " isn't a valid configuration.")
continue
grip_data = Float64()
grip_data.data = 1.5
self.pub_gripper.publish(grip_data)
rospy.sleep(2)
self.home_4()
rospy.sleep(2)
grip_data.data = 0.0
self.pub_gripper.publish(grip_data)
rospy.loginfo("End process")
return pick_srvResponse(True)
degree += 1
return pick_srvResponse(False)
def move(self, x, y, z):
pose_goal = Pose()
pose_goal.position.x = x
pose_goal.position.y = y
pose_goal.position.z = z
# ik_4dof.ik_solver(x, y, z, degree)
joint_value = ik_4dof.ik_solver(pose_goal.position.x,
pose_goal.position.y,
pose_goal.position.z, -90)
for joint in joint_value:
joint = list(joint)
# determine gripper state
joint.append(0)
try:
self.move_group.go(joint, wait=True)
except:
rospy.loginfo(str(joint) + " isn't a valid configuration.")
def processFeedback(self, feedback):
p = feedback.pose.position
self.tf_listener.waitForTransform("/base_link", "/world",
rospy.Time(0), rospy.Duration(1))
pp = PointStamped()
pp.point = p
pp.header.frame_id = "/world"
point_after = self.tf_listener.transformPoint("arm_base_link", pp)
x = point_after.point.x #+ random.uniform(0.005, -0.005)
y = point_after.point.y #+ random.uniform(0.005, -0.005)
# print "x: ",x, " y: ",y
z = point_after.point.z
ik_candidate = ik_solver(x, y, z, -90)
# print ik_candidate
if not np.isnan(ik_candidate.all()):
for theta_1, theta_2, theta_3, theta_4 in ik_candidate:
if abs(theta_2) < 2 and abs(theta_3) < 2 and abs(theta_4) < 2:
self.ik = [theta_1, theta_2, theta_3, theta_4]
break
def handle_move_arm(req):
# initial publisher for gripper command topic which is used for gripper control
pub_gripper = rospy.Publisher("/gripper_joint/command",
Float64,
queue_size=1)
check = True
# you need to check if moveit server is open or not
while (check):
check = False
try:
# Instantiate a MoveGroupCommander object. This object is an interface to a planning group
# In our case, we have "arm" and "gripper" group
move_group = moveit_commander.MoveGroupCommander("arm")
except:
print "moveit server isn't open yet"
check = True
# First initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
# Instantiate a RobotCommander object.
# Provides information such as the robot's kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
# We can get the name of the reference frame for this robot:
planning_frame = move_group.get_planning_frame()
print "============ Planning frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = move_group.get_end_effector_link()
print "============ End effector link: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Available Planning Groups:", group_names
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state", robot.get_current_state()
print ""
### Go home
home()
############################ Method : Using IK to calculate joint value ############################
# After determining a specific point where arm should move, we input x,y,z,degree to calculate joint value for each wrist.
pose_goal = Pose()
pose_goal.position.x = req.x
pose_goal.position.y = req.y
pose_goal.position.z = -0.06
# ik_4dof.ik_solver(x, y, z, degree)
joint_value = ik_4dof.ik_solver(pose_goal.position.x, pose_goal.position.y,
pose_goal.position.z, -90)
for joint in joint_value:
joint = list(joint)
# determine gripper state
joint.append(0)
try:
move_group.go(joint, wait=True)
except:
rospy.loginfo(str(joint) + " isn't a valid configuration.")
# Reference: https://ros-planning.github.io/moveit_tutorials/doc/move_group_python_interface/move_group_python_interface_tutorial.html
### close gripper
grip_data = Float64()
grip_data.data = 1.3
pub_gripper.publish(grip_data)
### Go home
home()
### open gripper
rospy.sleep(2)
grip_data = Float64()
grip_data.data = 0.5
pub_gripper.publish(grip_data)
rospy.sleep(2)
def __init__(self):
check = True
# you need to check if moveit server is open or not
while (check):
check = False
try:
# Instantiate a MoveGroupCommander object. This object is an interface to a planning group
# In our case, we have "arm" and "gripper" group
move_group = moveit_commander.MoveGroupCommander("arm")
except:
print "moveit server isn't open yet"
check = True
# First initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
# Instantiate a RobotCommander object.
# Provides information such as the robot's kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
# Instantiate a PlanningSceneInterface object.
# This provides a remote interface for getting, setting,
# and updating the robot's internal understanding of the surrounding world
scene = moveit_commander.PlanningSceneInterface()
# Create a DisplayTrajectory ROS publisher which is used to display trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the name of the reference frame for this robot:
planning_frame = move_group.get_planning_frame()
print "============ Planning frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = move_group.get_end_effector_link()
print "============ End effector link: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Available Planning Groups:", robot.get_group_names(
)
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state", robot.get_current_state()
print ""
############################ Method : Joint values (Go home!)############################
# We can get the joint values from the group and adjust some of the values:
# Go home!!!
joint_goal = move_group.get_current_joint_values()
joint_goal[0] = 0 # arm_shoulder_pan_joint
joint_goal[1] = -pi * 5 / 13 # arm_shoulder_lift_joint
joint_goal[2] = pi * 3 / 4 # arm_elbow_flex_joint
joint_goal[3] = pi / 3 # arm_wrist_flex_joint
joint_goal[4] = 0 # gripper_joint
# The go command can be called with joint values, poses, or without any
# parameters if you have already set the pose or joint target for the group
move_group.go(joint_goal, wait=True)
# Calling ``stop()`` ensures that there is no residual movement
move_group.stop()
############################ Method : Using IK to calculate joint value ############################
# After determining a specific point where arm should move, we input x,y,z,degree to calculate joint value for each wrist.
pose_goal = Pose()
pose_goal.position.x = 0.141
pose_goal.position.y = 0.020
pose_goal.position.z = 0.042
# ik_4dof.ik_solver(x, y, z, degree)
joint_value = ik_4dof.ik_solver(pose_goal.position.x,
pose_goal.position.y,
pose_goal.position.z, -90)
for joint in joint_value:
joint = list(joint)
# determine gripper state
joint.append(0)
try:
move_group.go(joint, wait=True)
except:
rospy.loginfo(str(joint) + " isn't a valid configuration.")
def transform(self, req):
br = tf.TransformBroadcaster()
tag = "tag_" + str(req.tag_id)
try:
now = rospy.Time.now()
self.listener.waitForTransform('pi_camera', tag, now,
rospy.Duration(3.0))
(trans, rot) = self.listener.lookupTransform('pi_camera', tag, now)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, \
tf.Exception):
rospy.loginfo("Tag not found!")
return tagResponse("Tag not found!")
pose_goal = geometry_msgs.msg.Pose()
pose_goal.position.x = trans[2] - 0.22
pose_goal.position.y = -trans[0]
pose_goal.position.z = trans[1] + 0.01
x1 = pose_goal.position.x
print "Your box's position : ", pose_goal.position
# if 0.15 <= pose_goal.position.x <= 0.21:
# if -0.07 <= pose_goal.position.y <= 0.065:
# self.special()
# self._return()
# return tagResponse("Process Successfully")
# elif -0.15 <= pose_goal.position.y <= 0.165:
# return tagResponse("Cannot arrive")
for l in range(6):
pose_goal.position.x = x1
for i in range(5):
degree = -90
for j in range(90):
joint_value = ik_4dof.ik_solver(pose_goal.position.x,
pose_goal.position.y,
pose_goal.position.z,
degree)
if len(joint_value) > 0:
for joint in joint_value:
joint = list(joint)
# determine gripper state
joint.append(0)
try:
self.move_group_arm.go(joint, wait=True)
self.move_group_arm.stop()
except:
rospy.loginfo(
str(joint) +
" isn't a valid configuration.")
continue
self._return()
return tagResponse("Process Successfully")
degree += 1
pose_goal.position.x -= 0.01
pose_goal.position.z += 0.01
return tagResponse("Cannot arrive objective pose!!")
