I've been trying to create my own ManipulationStation for a different robot arm using Pydrake, but I've been unsuccessful so far in adding clutter to my ManipulationStation. For some odd reason, Meshcat won't show the updated poses of my objects.
import numpy as np
import glob
from pydrake.geometry import MeshcatVisualizerCpp
from pydrake.math import RigidTransform, RotationMatrix
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder
from pydrake.all import (
DiagramBuilder, FindResourceOrThrow,
SceneGraph, Diagram,
MultibodyPlant, Parser, Simulator, MeshcatVisualizerCpp,
UniformlyRandomRotationMatrix, RandomGenerator)
from pydrake.geometry import Meshcat
class DexterPPStation(Diagram):
def __init__(self, time_step, file_path):
super().__init__()
self.time_step = time_step
self.path = file_path
self.plant = MultibodyPlant(self.time_step)
self.scene_graph = SceneGraph()
self.plant.RegisterAsSourceForSceneGraph(self.scene_graph)
self.controller_plant = MultibodyPlant(self.time_step)
self.object_ids = []
self.object_poses = []
def AddObject(self, file, name, pose):
model_idx = Parser(self.plant).AddModelFromFile(file, name)
indices = self.plant.GetBodyIndices(model_idx)
self.object_ids.append(indices[0])
self.object_poses.append(pose)
return model_idx
def CreateBins(self, path, XP_B1, XP_B2):
bin1 = Parser(self.plant).AddModelFromFile(path, "bin1")
self.plant.WeldFrames(self.plant.world_frame(), self.plant.GetFrameByName("bin_base", bin1), XP_B1)
bin2 = Parser(self.plant).AddModelFromFile(path, "bin2")
self.plant.WeldFrames(self.plant.world_frame(), self.plant.GetFrameByName("bin_base", bin2), XP_B2)
def CreateRandomPickingObjects(self, n = 4):
choices = [f for f in glob.glob("/opt/drake/share/drake/manipulation/models/ycb/sdf/*.sdf")]
z = 0.1
rs = np.random.RandomState()
generator = RandomGenerator(rs.randint(1000))
for i in range(n):
obj = choices[i]
pose = RigidTransform(
UniformlyRandomRotationMatrix(generator),
[rs.uniform(.35,0.6), rs.uniform(-.2, .2), z])
model = self.AddObject(obj, obj.split("/")[-1].split(".")[0] + str(i), pose)
body_idx = self.plant.GetBodyIndices(model)[0]
self.object_ids.append(body_idx)
self.object_poses.append(pose)
z+=0.1
def SetRandomPoses(self, station_context):
plant_context = self.GetSubsystemContext(self.plant, station_context)
for i in range(len(self.object_ids)):
self.plant.SetFreeBodyPose(plant_context, self.plant.get_body(self.object_ids[i]), self.object_poses[i])
def Finalize(self):
self.plant.Finalize()
self.controller_plant.Finalize()
builder = DiagramBuilder()
builder.AddSystem(self.plant)
builder.AddSystem(self.controller_plant)
builder.AddSystem(self.scene_graph)
builder.Connect(self.plant.get_geometry_poses_output_port(), self.scene_graph.get_source_pose_port(self.plant.get_source_id()))
builder.Connect(self.scene_graph.get_query_output_port(), self.plant.get_geometry_query_input_port())
builder.ExportOutput(self.scene_graph.get_query_output_port(), "query_object")
builder.ExportOutput(self.plant.get_geometry_poses_output_port(), "geometry_poses")
builder.ExportOutput(self.scene_graph.get_query_output_port(), "geometry_query")
builder.ExportOutput(self.plant.get_contact_results_output_port(),"contact_results")
builder.ExportOutput(self.plant.get_state_output_port(),"plant_continuous_state")
builder.BuildInto(self)
To test my code, I've been running the script below.
def test():
builder = DiagramBuilder()
station = DexterPPStation(1e-4, "drake/manipulation/models/final_dexter_description/urdf/dexter.urdf")
station.CreateBins("/opt/drake/share/drake/examples/manipulation_station/models/bin.sdf", RigidTransform(np.array([0.5,0,0])), RigidTransform(np.array([0,0.5,0])))
station.CreateRandomPickingObjects(1)
station.Finalize()
builder.AddSystem(station)
station_context = station.CreateDefaultContext()
station.SetRandomPoses(station_context)
MeshcatVisualizerCpp.AddToBuilder(builder, station.GetOutputPort("query_object"), meshcat)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(0.1)
test()
I've tried to call the SetRandomPoses() function from inside my Finalize() method, but since I needed to pass in a context to the function, I wasn't sure what to do. I'm new to Drake, so any input would be greatly appreciated.
You've created a station_context and set it to the random poses, but then you don't use it anywhere. When you create the simulator, it is creating another Context (with the default values), which is getting published when you call AdvanceTo.
The solution here, I think, is to not create your own station_context, but do e.g.
simulator = Simulator(diagram)
diagram_context = simulator.get_mutable_context()
station_context = station.GetMyMutableContextFromRoot(diagram_context)
station.SetRandomPoses(station_context)
then you can call AdvanceTo.
Related
I've been trying to create a TrajectorySource for RigidTransforms to pass into a DifferentialInverseKinematicsIntegrator which only takes in RigidTransforms in its input port.
def createTraj(time, pose):
times = []
poses = []
for step in time:
times.append(time[step])
poses.append(pose[step])
return PiecewisePose.MakeLinear(times, poses)
Initially, I tried to directly pass in the output from createTraj above into TrajectorySource but ran into the issue of my trajectory having more than one columns: Failure at systems/primitives/trajectory_source.cc:21 in TrajectorySource(): condition 'trajectory.cols() == 1' failed.
import matplotlib.pyplot as plt, mpld3
class DexterTest():
# Output from createTraj is passed as parameter: traj into constructor
def __init__(self, traj):
builder = DiagramBuilder()
self.station = DexterPPStation(1e-4, "/opt/drake/share/drake/manipulation/models/final_dexter_description/urdf/dexter.urdf")
self.station.CreateBins("/opt/drake/share/drake/examples/manipulation_station/models/bin.sdf", RigidTransform(np.array([0.5,0,0])), RigidTransform(np.array([0,0.5,0])))
self.station.CreateRandomPickingObjects(3)
self.station.AddDexter()
builder.AddSystem(self.station)
self.station.Finalize()
self.diff_ik = DifferentialInverseKinematicsIntegrator(self.station.controller_plant, self.station.plant.GetFrameByName("link6", self.station.dexter["instance"]), self.station.time_step, DifferentialInverseKinematicsParameters(7,7))
builder.AddSystem(self.diff_ik)
#=========================================== Likely Source of Error ===========================================
pose = builder.AddSystem(PoseSystem())
p_G_source = builder.AddSystem(TrajectorySource(traj.get_position_trajectory()))
w_G_source = builder.AddSystem(TrajectorySource(traj.get_orientation_trajectory()))
builder.Connect(p_G_source.get_output_port(), pose.GetInputPort("p_G"))
builder.Connect(w_G_source.get_output_port(), pose.GetInputPort("r_G"))
builder.Connect(pose.get_output_port(), self.diff_ik.get_input_port())
#======================================================================================
MeshcatVisualizerCpp.AddToBuilder(builder, self.station.GetOutputPort("query_object"), meshcat)
self.diagram = builder.Build()
self.simulator = Simulator(self.diagram)
self.diagram_context = self.simulator.get_mutable_context()
self.station_context = self.station.GetMyMutableContextFromRoot(self.diagram_context)
self.plant_context = self.station.GetSubsystemContext(self.station.plant, self.station_context)
self.station.SetRandomPoses(self.plant_context)
builder.Connect(self.diff_ik.get_output_port(), self.station.GetInputPort("dexter_position"))
def run(self):
self.simulator.set_target_realtime_rate(2.0)
self.simulator.AdvanceTo(1)
class PoseSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.p_G = self.DeclareVectorInputPort("p_G", BasicVector(3))
self.r_G = self.DeclareVectorInputPort("r_G", BasicVector(4))
self.DeclareAbstractOutputPort("X_G", Value[RigidTransform], self.CalcOutput)
def CalcOutput(self, context, output):
pose = RigidTransform(Quaternion(self.r_G.Eval(context)), self.p_G.Eval(context))
output.set_value(pose)
Instead, I tried to break up my trajectory into its orientation and position parts, add them to the input ports of a custom system, and then reconstruct them together in the output port. However, this gives me the following RuntimeError once the run method is called: RuntimeError: This multibody element does not belong to the supplied MultibodyTree.
Any help would be greatly appreciated!
I think you are very close. The PoseSystem looks like it should be a solution to the problem you've articulated in your post. (The error about MultibodyTree must be coming from the other part of your code.
You don't actually need to break the RigidTransform up into orientation / translation to create your PoseSystem, your CalcOutput could just call output.set_value(poses.Eval(t)) if poses is a PiecewisePose trajectory.
I have an example of doing this in the PickAndPlaceTrajectory class in this notebook: https://github.com/RussTedrake/manipulation/blob/008cec6343dd39063705287e6664a3fee71a43b8/pose.ipynb
Let's say we have a concurrent SMACH container sm_con which includes two state machines SM1 and SM2. I need to find a way for SM1 to continuously update some data and for SM2 to access (and eventually also modify) the same data. I thought about solving this by passing the userdata of sm_con to SM1 and SM2 as input- and output-keys hoping that if SM1 modifies the data it would automatically overwrite sm_cons userdata (kind of like working with pointers in c++) but this doesn't work.
The corresponding code would look like this:
import smach
import smach_ros
import rospy
class st1(smach.State):
def __init__(self, outcomes=['successful', 'preempted']):
smach.State.__init__(self, outcomes, input_keys=['in_test'], output_keys=['out_test'])
def execute(self, userdata):
if self.preempt_requested():
self.service_preempt()
return 'preempted'
rospy.logerr('test 1: '+str(userdata.in_test))
userdata.out_test=userdata.in_test+1
return 'successful'
class st2(smach.State):
def __init__(self, outcomes=['successful', 'preempted']):
smach.State.__init__(self, outcomes, input_keys=['in_test'])
def execute(self, userdata):
#time.sleep(2)
if self.preempt_requested():
self.service_preempt()
return 'preempted'
rospy.logerr('test 2: ' + str(userdata.in_test))
return 'successful'
if __name__=="__main__":
rospy.init_node('test_state_machine')
sm_con = smach.Concurrence(outcomes=['success'],
default_outcome='success'
)
sm_con.userdata.testdata = 0
with sm_con:
sm_1 = smach.StateMachine(outcomes=['success', 'preempted'], input_keys=['testdata'], output_keys=['testdata'])
with sm_1:
smach.StateMachine.add('ST1', st1(),
remapping={'in_test': 'testdata', 'out_test': 'testdata'},
transitions={'successful': 'ST1'})
sm_2 = smach.StateMachine(outcomes=['success', 'preempted'], input_keys=['testdata'])
with sm_2:
smach.StateMachine.add('ST2', st2(),
remapping={'in_test':'testdata'},
transitions={'successful': 'ST2'})
smach.Concurrence.add('SM1', sm_1)
smach.Concurrence.add('SM2', sm_2)
# Execute SMACH plan
outcome = sm_con.execute()
print('exit-outcome:' + outcome)
# Wait for ctrl-c to stop the application
rospy.spin()
Running this code, the output 'test 1: ...' shows that inside SM1 the userdata gets incremented while the output 'test 2: ...' shows that SM2 doesn't access the incremented data as the output remains 0.
How can I modify some data in SM1 and access the modified data in SM2?
I found a workaround for this using mutable objects like described here.
Applied on the code above, it would look like the following:
import smach
import smach_ros
import rospy
class st1(smach.State):
def __init__(self, outcomes=['successful', 'preempted']):
smach.State.__init__(self, outcomes, input_keys=['in_test'])
def execute(self, userdata):
if self.preempt_requested():
self.service_preempt()
return 'preempted'
rospy.logerr('test 1: '+str(userdata.in_test))
userdata.in_test[0]=userdata.in_test[0]+1
return 'successful'
class st2(smach.State):
def __init__(self, outcomes=['successful', 'preempted']):
smach.State.__init__(self, outcomes, input_keys=['in_test'])
def execute(self, userdata):
#time.sleep(2)
if self.preempt_requested():
self.service_preempt()
return 'preempted'
rospy.logerr('test 2: ' + str(userdata.in_test[0]))
return 'successful'
if __name__=="__main__":
rospy.init_node('test_state_machine')
sm_con = smach.Concurrence(outcomes=['success'],
default_outcome='success'
)
sm_con.userdata.testdata = [0]
with sm_con:
sm_1 = smach.StateMachine(outcomes=['success', 'preempted'], input_keys=['testdata'])
with sm_1:
smach.StateMachine.add('ST1', st1(),
remapping={'in_test': 'testdata'},
transitions={'successful': 'ST1'})
sm_2 = smach.StateMachine(outcomes=['success', 'preempted'], input_keys=['testdata'])
with sm_2:
smach.StateMachine.add('ST2', st2(),
remapping={'in_test':'testdata'},
transitions={'successful': 'ST2'})
smach.Concurrence.add('SM1', sm_1)
smach.Concurrence.add('SM2', sm_2)
# Execute SMACH plan
outcome = sm_con.execute()
print('exit-outcome:' + outcome)
# Wait for ctrl-c to stop the application
rospy.spin()
Since this is only a workaround, refer to my corresponding issue-post here for more information.
I really don't now why I can't call the method setTRSKey from inside my for loop. Am I missing something? This makes no sense to me at all. Pycharm declares it as an unsolved reference
Here is the code:
import math
import nuke
originNode = nuke.selectedNode()
world_matrix = originNode['world_matrix'] # this is an iArray Knob with 16 fields
mResult = nuke.math.Matrix4() # Matrix to copy iArray to
# Ask user for Frame Range operation
ret = nuke.getFramesAndViews('Frame range', '%s-%s' % (nuke.root().firstFrame(), nuke.root().lastFrame()))
if ret != None:
nuke.nodeCopy("%clipboard%") # creating node duplicate
originNode.setSelected(False)
newNode = nuke.nodePaste("%clipboard%") # creating origin node duplicate
newNode['translate'].clearAnimated()
newNode['translate'].setValue(0)
newNode['translate'].setAnimated()
newNode['rotate'].clearAnimated()
newNode['rotate'].setValue(0)
newNode['rotate'].setAnimated()
newNode['scaling'].clearAnimated()
newNode['scaling'].setValue(0)
newNode['scaling'].setAnimated()
frange = nuke.FrameRange(ret[0]) # convert to frange object
for frame in frange:
for i in xrange(0, 16):
mResult[i] = world_matrix.valueAt(frame)[i]
mResult.transpose() # row become columns and vice versa
mTranslate = nuke.math.Matrix4(mResult)
mTranslate.translationOnly()
mRotate = nuke.math.Matrix4(mResult)
mRotate.rotationOnly()
mScale = nuke.math.Matrix4(mResult)
mScale.scaleOnly()
translate = (mTranslate[12], mTranslate[13], mTranslate[14])
rotateRad = mRotate.rotationsZXY()
rotate = (math.degrees(rotateRad[0]), math.degrees(rotateRad[1]),
math.degrees(rotateRad[2])) # convert from radiants to defrees
scale = (mScale.xAxis().x, mScale.yAxis().y, mScale.zAxis().z)
setTRSKey(frame, translate, rotate, scale)
else:
print "User canceled the operation"
def setTRSKey(frame, translate, rotate, scale):
print type(translate(0))
newNode['translate'].setValueAt(translate(0), frame, 0)
newNode['translate'].setValueAt(translate(1), frame, 1)
newNode['translate'].setValueAt(translate(2), frame, 2)
edit: Example with classes where loadDataFromScript is called before defining
class Connecthor(QtWidgets.QDialog, Ui_Dialog):
#
allowedNodes = ["Read", "Write", "Merge", "Keymix", "ChannelMerge", "Roto", "RotoPaint", "Copy", "Shuffle", "PostageStamp", "Camera", "Camera2", "ScanlineRender", "Connector", "ReadGeo", "ReadGeo2", "BackdropNode"]
script_path = os.path.dirname(os.path.realpath(__file__))
#constructor
def __init__(self, parent=None):
super(Connecthor, self).__init__(parent)
self.setupUi(self)
self.setFixedSize(self.size())
#self.setWindowOpacity(0.95)
popupmenu = QtWidgets.QMenu(self.btn_settings)
#popupmenu.addAction("save links for script", self.writeListDictToJson)
#popupmenu.addAction("import links from json", self.readJsonToDict)
popupmenu.addAction("save links for script (manual)", self.saveDatatoScript)
popupmenu.addAction("settings", self.opensetting)
self.btn_settings.setMenu(popupmenu)
self.btn_settings.setIcon(QtGui.QIcon(os.path.join(iconpath, "settings.png")))
self.btn_addSelectedNodes.setIcon(QtGui.QIcon(os.path.join(iconpath, "add.png")))
self.btn_addSelectedNodes.clicked.connect(self.addSelectedNodes)
# #Loading test Json
#self.readJsonToDict()
self.loadDataFromScript()
In Python you must define functions before they are called. Move your setTRSKey definition above the for loop. Generally speaking, function definitions are one of the very first things in the file after imports, though this is not always the case.
I want to add rows to a datagridview "manually". I tried converting the following code to python: https://learn.microsoft.com/en-us/dotnet/framework/winforms/controls/how-to-manipulate-rows-in-the-windows-forms-datagridview-control
However, I struggle with adding rows. The following doesn't work:
for j in range(len(signals)):
self._dataGridView1.Rows.Add(signals[j])
The following code does work, but is not dynamically enough as I don't know how many elements there will be:
for j in range(len(signals)):
self._dataGridView1.Rows.Add(signals[j][0], signals[j][1], signals[j][2], signals[j][3])
How should I fix this? I tried tuple, but the result were a tuple with all the info shown in the first cell instead of spread over the columns.
I would not like to add packages, as this is to be run within revid dynamo among several users, and I cannot convince everyone to install packages.
full code for context:
import clr
clr.AddReference('System.Windows.Forms')
clr.AddReference('System.Drawing')
clr.AddReference('System.Data')
clr.AddReference('RevitAPIUI')
from Autodesk.Revit.UI import TaskDialog
from System.Windows.Forms import *
from System.Drawing import (
Point, Size,
Font, FontStyle,
GraphicsUnit
)
from System.Data import DataSet
from System.Data.Odbc import OdbcConnection, OdbcDataAdapter
msgBox = TaskDialog
headers = IN[0]
signals = IN[1]
class DataGridViewQueryForm(Form):
def __init__(self):
self.Text = 'Signals'
self.ClientSize = Size(942, 255)
self.MinimumSize = Size(500, 200)
self.setupDataGridView()
def setupDataGridView(self):
self._dataGridView1 = DataGridView()
self._dataGridView1.AllowUserToOrderColumns = True
self._dataGridView1.ColumnHeadersHeightSizeMode = DataGridViewColumnHeadersHeightSizeMode.AutoSize
self._dataGridView1.Dock = DockStyle.Fill
self._dataGridView1.Location = Point(0, 111)
self._dataGridView1.Size = Size(506, 273)
self._dataGridView1.TabIndex = 3
self._dataGridView1.ColumnCount = len(headers)
self._dataGridView1.ColumnHeadersVisible = True
for i in range(len(headers)):
self._dataGridView1.Columns[i].Name = headers[i]
for j in range(len(signals)):
self._dataGridView1.Rows.Add(signals[j][0], signals[j][1], signals[j][2], signals[j][3])
self.Controls.Add(self._dataGridView1)
Application.Run(DataGridViewQueryForm())
Figured it out. Had to use System.Array.
from System import Array
code changes:
array_str = Array.CreateInstance(str, len(headers))
for j in range(len(signals)):
for k in range(len(headers)):
array_str[k] = signals[j][k]
self._dataGridView1.Rows.Add(array_str)
Im writing a custom reader node for maya (in python with openmaya API 2.0) and I would like to send my uv sets to a regular maya mesh node.
Im wondering what would be the best way to push the uv sets in a mesh node? I wasn't able to find what data I have to create and how to send them to the mesh node.
My reader is a OpenMaya.MPxNode who push custom data to a OpenMaya.MPxSurfaceShape. The shape is linked by out mesh / in mesh plugs to a regular maya mesh. I attempt to fill the uvSet plug of this shape using compute but without success. I expect the UVSets to be sent to the mesh.
The following code sample is a limited test where my only goal is to create a new UVSet and attach it to the mesh.
Any ideas or documentation which could help?
I tried several things but I always get the error below.
The error:
// Error: (kFailure): Object does not exist
# Traceback (most recent call last):
# File "/path/to/maya/plug-ins/test_create_uv_set.py", line 60, in compute
# mesh.createUVSet("toto")
# RuntimeError: (kFailure): Object does not exist //
The running code:
"""
usage
import maya.cmds as cmds
import pymel.core as pm
cmds.loadPlugin("/path/to/maya/plug-ins/test_create_uv_set.py")
transform_node = pm.polySphere(n='transform1', ch=1, o=1, r=4)[0]
mesh1_node = transform_node.getShape()
pm.setAttr(mesh1_node + ".visibility", False)
uv_set_mod_node = pm.createNode("uvSetModifier", name="uvsetmodifier1")
mesh2_node = pm.createNode("mesh", name="mesh2", parent=transform_node)
pm.hyperShade(assign="initialShadingGroup")
pm.Attribute.connect(mesh1_node.attr("outMesh"), uv_set_mod_node.attr("inMesh"))
pm.Attribute.connect(uv_set_mod_node.attr("outMesh"), mesh2_node.attr("inMesh"))
"""
import sys
import maya.api.OpenMaya as OpenMaya
def maya_useNewAPI():
pass
class uvSetModifier(OpenMaya.MPxNode):
typeName = "uvSetModifier"
id = OpenMaya.MTypeId(0xCCCCC)
inMesh = None
outMesh = None
#staticmethod
def creator():
return uvSetModifier()
#staticmethod
def initialize():
typedAttr = OpenMaya.MFnTypedAttribute()
uvSetModifier.inMesh = typedAttr.create("inMesh", "im", OpenMaya.MFnData.kMesh)
typedAttr.writable = True
OpenMaya.MPxNode.addAttribute(uvSetModifier.inMesh)
uvSetModifier.outMesh = typedAttr.create("outMesh", "om", OpenMaya.MFnData.kMesh)
typedAttr.writable = True
OpenMaya.MPxNode.addAttribute(uvSetModifier.outMesh)
def __init__(self):
OpenMaya.MPxNode.__init__(self)
def compute(self, plug, datablock):
if plug == uvSetModifier.outMesh:
inputData = datablock.inputValue(uvSetModifier.inMesh)
outputData = datablock.outputValue(uvSetModifier.outMesh)
outputData.setMObject(inputData.asMesh())
mesh = OpenMaya.MFnMesh(inputData.asMesh())
mesh.createUVSet("toto")
datablock.setClean(plug)
def initializePlugin(obj):
plugin = OpenMaya.MFnPlugin(obj, "Autodesk", "3.0", "Any")
try:
plugin.registerNode(uvSetModifier.typeName, uvSetModifier.id, uvSetModifier.creator, uvSetModifier.initialize)
except:
sys.stderr.write("Failed to register node\n")
raise
def uninitializePlugin(obj):
plugin = OpenMaya.MFnPlugin(obj)
try:
plugin.deregisterNode(uvSetModifier.id)
except:
sys.stderr.write("Failed to deregister node\n")
pass
UPDATE 1: THEODOX input
I added the following line (59)
if inputData.asMesh() is not None:
print "test"
mesh = OpenMaya.MFnMesh(inputData.asMesh())
mesh.createUVSet("toto")
results: I still get the same error message