def motion(self, x, y, z, roll, pitch, yaw): manipu_init_pose = self.manipu.get_current_pose().pose self.x = x self.y = y self.z = z + 0.006 self.roll = math.radians(roll) self.pitch = math.radians(pitch) self.yaw = math.radians(yaw) self.q = tf.transformations.quaternion_from_euler( self.roll, self.pitch, self.yaw) self.target_pose.position.x = self.x self.target_pose.position.y = self.y self.target_pose.position.z = self.z self.target_pose.orientation.x = self.q[0] self.target_pose.orientation.y = self.q[1] self.target_pose.orientation.z = self.q[2] self.target_pose.orientation.w = self.q[3] self.manipu.set_pose_target(self.target_pose) plan = self.manipu.plan() if self.manipu.execute(plan): print "This plan is succes!" joint_msg = rospy.Publisher('/robotis/base/joint_pose_msg', JointPose, queue_size=100) jointpose = JointPose() jointpose.name = plan.joint_trajectory.joint_names start = time.time() jointpose.value = plan.joint_trajectory.points[0].positions joint_msg.publish(jointpose) plan_count = 1 while (plan_count < len(plan.joint_trajectory.points)): if ((time.time() - start) >= 0.8): jointpose.value = plan.joint_trajectory.points[ plan_count].positions joint_msg.publish(jointpose) plan_count = plan_count + 1 start = time.time() print "finished to send all jointTrajectory" else: print "Thin plan is NOT succes" return
def jointMove(self, speed, slide_pos=0, cmd=[0, 0, 0, 0, 0, 0, 0]): """Publish msg of joint cmd (rad) to manager node.""" name = list() value = list() for i, val in enumerate(cmd): name.append('joint{}'.format(i + 1)) value.append(radians(val)) self.__joint_pub.publish(self.joint_topic, JointPose(name, value, slide_pos, speed))
def jointMove(self, slide_pos = 0,cmd=[0, 0, 0, 0, 0, 0, 0]): """Publish msg of joint cmd (rad) to manager node.""" name = list() value = list() speed = self.__speed for i, val in enumerate(cmd): name.append('joint{}'.format(i+1)) value.append(val) self.__joint_pub.publish(JointPose(name, value, slide_pos, speed)) self.__is_busy = True