Can anyone please help me with my code

asked 2019-09-11 06:05:15 -0500

Adwait_Naik gravatar image

updated 2019-09-11 07:20:14 -0500

gvdhoorn gravatar image

This is a code for circular path planning of ur5 arm in python. Its not working please help me debug it.

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
import pyassimp

#Declaring a class and initializing moveit headers
class JustTryThings(object):
    def __init__(self):
        super(JustTryThings, self).__init__()

        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('move_group_python_interface_tutorial',anonymous=True)

        robot = moveit_commander.RobotCommander()
        scene = moveit_commander.PlanningSceneInterface()
        group_name = "manipulator"
        group = moveit_commander.MoveGroupCommander(group_name)
        display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) 

        #details about robot:
        planning_frame = group.get_planning_frame()
        eef_link = group.get_end_effector_link()
        group_names = robot.get_group_names()

        #Declare variables globally:
        self.box_name = ''
        self.sphere_name = ''
        self.robot = robot
        self.scene = scene
        self.group = group
        self.display_trajectory_publisher = display_trajectory_publisher
        self.planning_frame = planning_frame
        self.eef_link = eef_link
        self.group_names = group_names
        self.pose_goal = geometry_msgs.msg.Pose()

        print('Setup finished')

        #Home position
    def homeposition(self):        
        group = self.group
        robot = self.robot
        scene = self.scene
        group.set_joint_value_target([0,-1.5707,1.5707,0,0,0])
        group.go()
        group.stop()
        #self.addboxtoworld(0.5,0,0,0,0.6,0.8,0.05,"box")
        eef_link = self.eef_link
        #self.addboxtoworld(0.40,0.215,0.41,0,0.05,0.05,0.05,"probe")
        grasping_group = 'manipulator'
        touch_links = robot.get_link_names(group=grasping_group)
        scene.attach_box(eef_link, "probe", touch_links=touch_links)

        #Add Box
    def addboxtoworld(self,pos_x,pos_y,pos_z,oren,box_l,box_b,box_w,object_name):
        group = self.group
        scene = self.scene
        eef_link = self.eef_link
        robot = self.robot
        # #Dummy home position

        #adding cube:
        box_pose = geometry_msgs.msg.PoseStamped()
        box_pose.header.frame_id = "/world"
        box_pose.pose.orientation.x = 0
        box_pose.pose.orientation.y = 0
        box_pose.pose.orientation.z = 1
        box_pose.pose.orientation.w = oren*pi/180.
        box_pose.pose.position.x = pos_x#0.4
        box_pose.pose.position.y = pos_y#0.1
        box_pose.pose.position.z = pos_z#0.4
        # box_name = object_name
        scene.add_box(object_name, box_pose, size = (box_l,box_b,box_w))
        # self.box_name = box_name
        print('BOX Added')

#Objects for above Class
key = JustTryThings()

#import Group and Scene for manipulating the robot and adding objects in 3D scene
Scene = key.scene
Group = key.group

# NOTE:
# Try using the above Scene and Group objects to maniplute robot and add objects n the world
# Refer to addboxtoworld and homeposition dunction in the class to play around the robot

#Main
key.homeposition()
key.addboxtoworld()

key.group.set_joint_value_target([0,0,0,0,0,0])
key.group.go()
key.group.stop()
edit retag flag offensive close merge delete

Comments

3

Please don't just ask us to do your work for you. Post a descriptive list of what you've tried, what's not working as expected, and what you're expecting to have happen. We can't do anything with "its broken" and a generic moveit script.

stevemacenski gravatar imagestevemacenski ( 2019-09-11 07:55:49 -0500 )edit
2

In addition to this please edit the question topic with something that actually reflects the problem you are facing

pavel92 gravatar imagepavel92 ( 2019-09-11 08:19:42 -0500 )edit