Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Create functions for grasp compute #1024

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Create functions for grasp compute #1024

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
b36c6b9
Deduplicate orientation copying code.
alberth Mar 10, 2020
9bc3966
Deduplicate position copying code.
alberth Mar 10, 2020
fff1602
Use class prefix for static methods
alberth Mar 14, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 17 additions & 19 deletions robot_skills/src/robot_skills/arms.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -521,31 +521,16 @@ def send_goal(self, frameStamped, timeout=30, pre_grasp=False, first_joint_pos_o

grasp_precompute_goal.allowed_touch_objects = allowed_touch_objects

grasp_precompute_goal.goal.x = frame_in_baselink.frame.p.x()
grasp_precompute_goal.goal.y = frame_in_baselink.frame.p.y()
grasp_precompute_goal.goal.z = frame_in_baselink.frame.p.z()

roll, pitch, yaw = frame_in_baselink.frame.M.GetRPY()
grasp_precompute_goal.goal.roll = roll
grasp_precompute_goal.goal.pitch = pitch
grasp_precompute_goal.goal.yaw = yaw

Arm.set_grasp_goal_position(grasp_precompute_goal.goal, frame_in_baselink.p)
Arm.set_grasp_goal_orientation(grasp_precompute_goal.goal, frame_in_baselink.frame.M.GetRPY())
self._publish_marker(grasp_precompute_goal, [1, 0, 0], "grasp_point")

# Add tunable parameters
offset_frame = frame_in_baselink.frame * self.offset

grasp_precompute_goal.goal.x = offset_frame.p.x()
grasp_precompute_goal.goal.y = offset_frame.p.y()
grasp_precompute_goal.goal.z = offset_frame.p.z()

roll, pitch, yaw = frame_in_baselink.frame.M.GetRPY()
grasp_precompute_goal.goal.roll = roll
grasp_precompute_goal.goal.pitch = pitch
grasp_precompute_goal.goal.yaw = yaw

Arm.set_grasp_goal_position(grasp_precompute_goal.goal, offset_frame.p)
Arm.set_grasp_goal_orientation(grasp_precompute_goal.goal, frame_in_baselink.frame.M.GetRPY())
# rospy.loginfo("Arm goal: {0}".format(grasp_precompute_goal))

self._publish_marker(grasp_precompute_goal, [0, 1, 0], "grasp_point_corrected")

time.sleep(0.001) # This is necessary: the rtt_actionlib in the hardware seems
Expand Down Expand Up @@ -583,6 +568,19 @@ def send_goal(self, frameStamped, timeout=30, pre_grasp=False, first_joint_pos_o
rospy.logerr('grasp precompute goal failed: \n%s', repr(myargs))
return False

@staticmethod
def set_grasp_goal_position(goal, pos):
goal.x = pos.x()
goal.y = pos.y()
goal.z = pos.z()

@staticmethod
def set_grasp_goal_orientation(goal, orient):
roll, pitch, yaw = orient
goal.roll = roll
goal.pitch = pitch
goal.yaw = yaw

def send_joint_goal(self, configuration, timeout=5.0, max_joint_vel=0.7):
"""
Send a named joint goal (pose) defined in the parameter default_configurations to the arm
Expand Down