def move_home(self,req): if req.home: self.MOVING = False self.x_v = 0 self.y_v = 0 self.z_v = 0 joint_angle_client([self.home_angles.joint1, self.home_angles.joint2, self.home_angles.joint3, self.home_angles.joint4, self.home_angles.joint5, self.home_angles.joint6, self.home_angles.joint7]) time.sleep(0.5) print('home!') self.MOVING = True return 1
def robot_wrench_callback(self, msg): if abs(msg.wrench.force.x) > 5.0 or abs( msg.wrench.force.y) > 5.0 or abs(msg.wrench.force.z) > 7.0: rospy.logerr('Force Detected. Stopping.') self.MOVING = False print(msg.wrench) print(self.temp_angles) if self.temp_angles: joint_angle_client([ self.temp_angles.joint1, self.temp_angles.joint2, self.temp_angles.joint3, self.temp_angles.joint4, self.temp_angles.joint5, self.temp_angles.joint6, self.temp_angles.joint7 ]) rospy.ServiceProxy('/j2s7s300_driver/in/start_force_control', kinova_msgs.srv.Start)