device.after.addSignal(signal) I4 = ( (1., 0, 0, 0), (0, 1., 0, 0), (0, 0, 1., 0), (0, 0, 0, 1.), ) model = RosRobotModel('ur5_dynamic') device = RobotSimu('ur5_device') rospy.init_node('fake') model.loadUrdf( "file:///local/ngiftsun/devel/ros/catkin_ws/src/ur_description/urdf/ur5_robot.urdf" ) dimension = model.getDimension() device.resize(dimension) plug(device.state, model.position) # Set velocity and acceleration to 0 model.velocity.value = dimension * (0., ) model.acceleration.value = dimension * (0., ) # Create taks for the base model.createOpPoint("base", "waist") # Create task for the wrist model.createOpPoint("wrist", "wrist_3_joint")
signal = '{0}.{1}'.format(entityName, signalName) filename = '{0}-{1}'.format(entityName, signalName) trace.add(signal, filename) if autoRecompute: device.after.addSignal(signal) I4 = ((1.,0,0,0), (0,1.,0,0), (0,0,1.,0), (0,0,0,1.),) model = RosRobotModel ('ur5_dynamic') device = RobotSimu ('ur5_device') rospy.init_node('fake') model.loadUrdf ("file:///local/ngiftsun/devel/ros/catkin_ws/src/ur_description/urdf/ur5_robot.urdf") dimension = model.getDimension () device.resize (dimension) plug (device.state, model.position) # Set velocity and acceleration to 0 model.velocity.value = dimension * (0.,) model.acceleration.value = dimension * (0.,) # Create taks for the base model.createOpPoint ("base", "waist") # Create task for the wrist model.createOpPoint ("wrist", "wrist_3_joint") feature_wrist = FeaturePosition ('position_wrist', model.wrist, model.Jwrist, I4)