Can anyone please help me with my code

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

Adwait_Naik gravatar image

updated 2022-01-22 16:10:04 -0600

Evgeny 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__()


        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 = 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 =
        robot = self.robot
        scene = self.scene
        eef_link = self.eef_link
        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 =
        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 =

# 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

edit retag flag offensive close merge delete



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 image stevemacenski  ( 2019-09-11 07:55:49 -0600 )edit

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

pavel92 gravatar image pavel92  ( 2019-09-11 08:19:42 -0600 )edit