def move_arm(self, positions): self._arm_goal = copy.deepcopy(positions) jp = YoubotProxy.make_brics_msg_arm(self.arm_num, positions) rospy.logdebug("brics message created") rospy.logdebug(jp) r = rospy.Rate(50) t0 = rospy.Time().now() pubbed = False while not rospy.is_shutdown(): curr_jpos = self._joint_positions_arm d = BaseProxy.measure_joint_distance_sum(curr_jpos, positions) v = BaseProxy.measure_joint_velocity_ss(self._joint_velocities_arm) rospy.logdebug(d) rospy.logdebug("arm joint positions") rospy.logdebug(curr_jpos) rospy.logdebug(d) rospy.logdebug("arm joint velocities") rospy.logdebug(self._joint_velocities_arm) if ((d < self.arm_joint_distance_tol) and (v<=self.arm_joint_velocity_tol)): # rospy.logdebug("moved arm to joint position error of: " + str(d)) break td = rospy.Time().now() if (td-t0 > self.arm_move_duration): rospy.logerr("move arm timeout") raise Exception("move arm timeout") break self._arm_pub.publish(jp) # if not pubbed: # self._arm_pub.publish(jp) # pubbed = True r.sleep()
def move_arm(self, positions): self._arm_goal = copy.deepcopy(positions) jp = YoubotProxy.make_brics_msg_arm(self.arm_num, positions) rospy.logdebug("brics message created") rospy.logdebug(jp) r = rospy.Rate(50) t0 = rospy.Time().now() pubbed = False while not rospy.is_shutdown(): curr_jpos = self._joint_positions_arm d = BaseProxy.measure_joint_distance_sum(curr_jpos, positions) v = BaseProxy.measure_joint_velocity_ss(self._joint_velocities_arm) rospy.logdebug(d) rospy.logdebug("arm joint positions") rospy.logdebug(curr_jpos) rospy.logdebug(d) rospy.logdebug("arm joint velocities") rospy.logdebug(self._joint_velocities_arm) if ((d < self.arm_joint_distance_tol) and (v <= self.arm_joint_velocity_tol)): # rospy.logdebug("moved arm to joint position error of: " + str(d)) break td = rospy.Time().now() if (td - t0 > self.arm_move_duration): rospy.logerr("move arm timeout") raise Exception("move arm timeout") break self._arm_pub.publish(jp) # if not pubbed: # self._arm_pub.publish(jp) # pubbed = True r.sleep()